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

Rover: converted to AP_ServoRelayEvents

master
Andrew Tridgell 11 years ago
parent
cdd36b2c41
commit
0828c40870
5 changed files with 67 additions and 100 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. 21
      APMrover2/APMrover2.pde
  2. 52
      APMrover2/GCS_Mavlink.pde
  3. 67
      APMrover2/commands_logic.pde
  4. 25
      APMrover2/events.pde
  5. 2
      APMrover2/system.pde

21
APMrover2/APMrover2.pde
Unescape View File

@ -79,6 +79,7 @@ @@ -79,6 +79,7 @@
#include <ModeFilter.h> // Mode Filter from Filter library
#include <AverageFilter.h> // Mode Filter from Filter library
#include <AP_Relay.h> // APM relay
#include <AP_ServoRelayEvents.h>
#include <AP_Mount.h> // Camera/Antenna mount
#include <AP_Camera.h> // Camera triggering
#include <GCS_MAVLink.h> // MAVLink GCS definitions
@ -293,6 +294,8 @@ static AP_RangeFinder_analog sonar2; @@ -293,6 +294,8 @@ static AP_RangeFinder_analog sonar2;
// relay support
AP_Relay relay;
AP_ServoRelayEvents ServoRelayEvents(relay);
// Camera
#if CAMERA == ENABLED
static AP_Camera camera(&relay);
@ -467,22 +470,6 @@ static float wp_distance; @@ -467,22 +470,6 @@ static float wp_distance;
// Distance between previous and next waypoint. Meters
static int32_t wp_totalDistance;
////////////////////////////////////////////////////////////////////////////////
// repeating event control
////////////////////////////////////////////////////////////////////////////////
// Flag indicating current event type
static uint8_t event_id;
// when the event was started in ms
static int32_t event_timer;
// how long to delay the next firing of event in millis
static uint16_t event_delay;
// 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
////////////////////////////////////////////////////////////////////////////////
@ -574,7 +561,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = { @@ -574,7 +561,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
{ read_trim_switch, 5, 1000 },
{ read_battery, 5, 1000 },
{ read_receiver_rssi, 5, 1000 },
{ update_events, 15, 1000 },
{ update_events, 1, 1000 },
{ check_usb_mux, 15, 1000 },
{ mount_update, 1, 600 },
{ gcs_failsafe_check, 5, 600 },

52
APMrover2/GCS_Mavlink.pde
Unescape View File

@ -1142,9 +1142,27 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) @@ -1142,9 +1142,27 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
case MAV_CMD_DO_SET_SERVO:
hal.rcout->enable_ch(packet.param1 - 1);
hal.rcout->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:
@ -1276,9 +1294,19 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) @@ -1276,9 +1294,19 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
param1 = tell_command.p1;
break;
case MAV_CMD_DO_REPEAT_SERVO:
param4 = tell_command.lng;
case MAV_CMD_DO_REPEAT_RELAY:
case MAV_CMD_DO_REPEAT_SERVO:
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;
@ -1567,8 +1595,18 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) @@ -1567,8 +1595,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;

67
APMrover2/commands_logic.pde
Unescape View File

@ -66,21 +66,23 @@ static void handle_process_do_command() @@ -66,21 +66,23 @@ static void handle_process_do_command()
do_set_home();
break;
case MAV_CMD_DO_SET_SERVO:
do_set_servo();
break;
case MAV_CMD_DO_SET_SERVO:
ServoRelayEvents.do_set_servo(next_nonnav_command.p1, next_nonnav_command.alt);
break;
case MAV_CMD_DO_SET_RELAY:
do_set_relay();
break;
case MAV_CMD_DO_SET_RELAY:
ServoRelayEvents.do_set_relay(next_nonnav_command.p1, next_nonnav_command.alt);
break;
case MAV_CMD_DO_REPEAT_SERVO:
do_repeat_servo();
break;
case MAV_CMD_DO_REPEAT_SERVO:
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();
break;
case MAV_CMD_DO_REPEAT_RELAY:
ServoRelayEvents.do_repeat_relay(next_nonnav_command.p1, next_nonnav_command.alt,
next_nonnav_command.lat);
break;
#if CAMERA == ENABLED
case MAV_CMD_DO_CONTROL_VIDEO: // Control on-board camera capturing. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty|
@ -366,47 +368,6 @@ static void do_set_home() @@ -366,47 +368,6 @@ static void do_set_home()
}
}
static void do_set_servo()
{
hal.rcout->enable_ch(next_nonnav_command.p1 - 1);
hal.rcout->write(next_nonnav_command.p1 - 1, next_nonnav_command.alt);
}
static void do_set_relay()
{
if (next_nonnav_command.p1 == 1) {
relay.on(0);
} else if (next_nonnav_command.p1 == 0) {
relay.off(0);
}else{
relay.toggle(0);
}
}
static void do_repeat_servo()
{
event_id = next_nonnav_command.p1 - 1;
if(next_nonnav_command.p1 >= CH_5 + 1 && next_nonnav_command.p1 <= CH_8 + 1) {
event_timer = 0;
event_delay = 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;
event_undo_value = RC_Channel::rc_channel(next_nonnav_command.p1-1)->radio_trim;
update_events();
}
}
static void do_repeat_relay()
{
event_id = RELAY_TOGGLE;
event_timer = 0;
event_delay = next_nonnav_command.lat * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds)
event_repeat = next_nonnav_command.alt * 2;
update_events();
}
// do_take_picture - take a picture with the camera library
static void do_take_picture()
{

25
APMrover2/events.pde
Unescape View File

@ -1,28 +1,7 @@ @@ -1,28 +1,7 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
static void update_events(void) // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY
static void update_events(void)
{
if(event_repeat == 0 || (millis() - event_timer) < event_delay)
return;
if (event_repeat > 0){
event_repeat --;
}
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);
}
}
ServoRelayEvents.update_events();
}

2
APMrover2/system.pde
Unescape View File

@ -106,6 +106,8 @@ static void init_ardupilot() @@ -106,6 +106,8 @@ static void init_ardupilot()
BoardConfig.init();
ServoRelayEvents.set_channel_mask(0xFFF0);
set_control_channels();
// after parameter load setup correct baud rate on uartA

Write Preview
Loading…
Cancel
Save