Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

alignment with APM 2.0

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1772 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-03-13 06:25:38 +00:00
parent 1c9635b058
commit e5fe9231f3
13 changed files with 635 additions and 554 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
ArduCopterMega/ArduCopterMega.pde
View File

@ -303,8 +303,6 @@ long old_alt; // used for managing altitude rates
int velocity_land;
bool nav_yaw_towards_wp; // point at the next WP
// Loiter management
// -----------------
long old_target_bearing; // deg * 100
@ -351,8 +349,9 @@ byte undo_event; // counter for timing the undo
// delay command
// --------------
int delay_timeout; // used to delay commands
long delay_start; // used to delay commands
long condition_value; // used in condition commands (eg delay, change alt, etc.)
long condition_start;
int condition_rate;
// 3D Location vectors
// -------------------

2
ArduCopterMega/Attitude.pde
View File

@ -159,7 +159,7 @@ output_yaw_with_hold(boolean hold)
g.rc_4.servo_out = g.pid_acro_rate_yaw.get_pid(error, delta_ms_fast_loop, 1.0);// kP .07 * 36000 = 2520
}
g.rc_4.servo_out = constrain(g.rc_4.servo_out, -2400, 2400); // limit to 24°
g.rc_4.servo_out = constrain(g.rc_4.servo_out, -1800, 1800); // limit to 24°
}
}

289
ArduCopterMega/GCS_Mavlink.pde
View File

@ -77,8 +77,9 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax)
send_message(MSG_GPS_RAW); // TODO - remove this message after location message is working
}
if (freqLoopMatch(global_data.streamRatePosition,freqMin,freqMax))
send_message(MSG_LOCATION);
if (freqLoopMatch(global_data.streamRatePosition,freqMin,freqMax)) {
send_message(MSG_LOCATION);
}
if (freqLoopMatch(global_data.streamRateRawController,freqMin,freqMax)) {
// This is used for HIL. Do not change without discussing with HIL maintainers
@ -90,11 +91,13 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax)
send_message(MSG_RADIO_IN);
}
if (freqLoopMatch(global_data.streamRateExtra1,freqMin,freqMax)) // Use Extra 1 for AHRS info
send_message(MSG_ATTITUDE);
if (freqLoopMatch(global_data.streamRateExtra1,freqMin,freqMax)){ // Use Extra 1 for AHRS info
send_message(MSG_ATTITUDE);
}
if (freqLoopMatch(global_data.streamRateExtra2,freqMin,freqMax)) // Use Extra 3 for additional HIL info
if (freqLoopMatch(global_data.streamRateExtra2,freqMin,freqMax)){ // Use Extra 2 for additional HIL info
send_message(MSG_VFR_HUD);
}
if (freqLoopMatch(global_data.streamRateExtra3,freqMin,freqMax)){
// Available datastream
@ -121,6 +124,7 @@ GCS_MAVLINK::acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2)
void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
{
struct Location tell_command; // command for telemetry
switch (msg->msgid) {
case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
@ -136,48 +140,48 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
else if (packet.start_stop == 1) freq = packet.req_message_rate; // start sending
else break;
switch(packet.req_stream_id)
{
case MAV_DATA_STREAM_ALL:
global_data.streamRateRawSensors = freq;
global_data.streamRateExtendedStatus = freq;
global_data.streamRateRCChannels = freq;
global_data.streamRateRawController = freq;
global_data.streamRatePosition = freq;
global_data.streamRateExtra1 = freq;
global_data.streamRateExtra2 = freq;
global_data.streamRateExtra3 = freq;
break;
case MAV_DATA_STREAM_RAW_SENSORS:
global_data.streamRateRawSensors = freq;
break;
case MAV_DATA_STREAM_EXTENDED_STATUS:
global_data.streamRateExtendedStatus = freq;
break;
case MAV_DATA_STREAM_RC_CHANNELS:
global_data.streamRateRCChannels = freq;
break;
case MAV_DATA_STREAM_RAW_CONTROLLER:
global_data.streamRateRawController = freq;
break;
//case MAV_DATA_STREAM_RAW_SENSOR_FUSION:
// global_data.streamRateRawSensorFusion = freq;
// break;
case MAV_DATA_STREAM_POSITION:
global_data.streamRatePosition = freq;
break;
case MAV_DATA_STREAM_EXTRA1:
global_data.streamRateExtra1 = freq;
break;
case MAV_DATA_STREAM_EXTRA2:
global_data.streamRateExtra2 = freq;
break;
case MAV_DATA_STREAM_EXTRA3:
global_data.streamRateExtra3 = freq;
break;
default:
break;
}
switch(packet.req_stream_id){
case MAV_DATA_STREAM_ALL:
global_data.streamRateRawSensors = freq;
global_data.streamRateExtendedStatus = freq;
global_data.streamRateRCChannels = freq;
global_data.streamRateRawController = freq;
global_data.streamRatePosition = freq;
global_data.streamRateExtra1 = freq;
global_data.streamRateExtra2 = freq;
global_data.streamRateExtra3 = freq;
break;
case MAV_DATA_STREAM_RAW_SENSORS:
global_data.streamRateRawSensors = freq;
break;
case MAV_DATA_STREAM_EXTENDED_STATUS:
global_data.streamRateExtendedStatus = freq;
break;
case MAV_DATA_STREAM_RC_CHANNELS:
global_data.streamRateRCChannels = freq;
break;
case MAV_DATA_STREAM_RAW_CONTROLLER:
global_data.streamRateRawController = freq;
break;
//case MAV_DATA_STREAM_RAW_SENSOR_FUSION:
// global_data.streamRateRawSensorFusion = freq;
// break;
case MAV_DATA_STREAM_POSITION:
global_data.streamRatePosition = freq;
break;
case MAV_DATA_STREAM_EXTRA1:
global_data.streamRateExtra1 = freq;
break;
case MAV_DATA_STREAM_EXTRA2:
global_data.streamRateExtra2 = freq;
break;
case MAV_DATA_STREAM_EXTRA3:
global_data.streamRateExtra3 = freq;
break;
default:
break;
}
break;
}
@ -205,25 +209,24 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
case MAV_ACTION_HALT:
do_hold_position();
do_loiter_at_location();
break;
// can't implement due to APM configuration
// just setting to manual to be safe
/* No mappable implementation in APM 2.0
case MAV_ACTION_MOTORS_START:
case MAV_ACTION_CONFIRM_KILL:
case MAV_ACTION_EMCY_KILL:
case MAV_ACTION_MOTORS_STOP:
case MAV_ACTION_SHUTDOWN:
set_mode(ACRO);
break;
*/
case MAV_ACTION_CONTINUE:
process_next_command();
break;
case MAV_ACTION_SET_MANUAL:
set_mode(ACRO);
set_mode(STABILIZE);
break;
case MAV_ACTION_SET_AUTO:
@ -247,31 +250,37 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAV_ACTION_CALIBRATE_ACC:
case MAV_ACTION_CALIBRATE_PRESSURE:
case MAV_ACTION_REBOOT: // this is a rough interpretation
startup_ground();
startup_IMU_ground();
break;
/* For future implemtation
case MAV_ACTION_REC_START: break;
case MAV_ACTION_REC_PAUSE: break;
case MAV_ACTION_REC_STOP: break;
*/
/* Takeoff is not an implemented flight mode in APM 2.0
case MAV_ACTION_TAKEOFF:
//set_mode(TAKEOFF);
set_mode(TAKEOFF);
break;
*/
case MAV_ACTION_NAVIGATE:
set_mode(AUTO);
break;
/* Land is not an implemented flight mode in APM 2.0
case MAV_ACTION_LAND:
//set_mode(LAND);
set_mode(LAND);
break;
*/
case MAV_ACTION_LOITER:
//set_mode(LOITER);
set_mode(LOITER);
break;
default: break;
}
}
break;
}
@ -318,33 +327,62 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// set frame of waypoint
uint8_t frame = MAV_FRAME_GLOBAL; // reference frame
uint8_t command = tell_command.id; // command
float param1 = 0, param2 = 0 , param3 = 0, param4 = 0;
param1 = tell_command.p1;
// time that the mav should loiter in milliseconds
uint8_t current = 0; // 1 (true), 0 (false)
if (packet.seq == g.waypoint_index) current = 1;
uint8_t autocontinue = 1; // 1 (true), 0 (false)
float x = 0, y = 0, z = 0;
if (command < MAV_CMD_NAV_LAST) {
if (tell_command.id < MAV_CMD_NAV_LAST) {
// command needs scaling
x = tell_command.lat/1.0e7; // local (x), global (longitude)
y = tell_command.lng/1.0e7; // local (y), global (latitude)
x = tell_command.lat/1.0e7; // local (x), global (latitude)
y = tell_command.lng/1.0e7; // local (y), global (longitude)
z = tell_command.alt/1.0e2; // local (z), global (altitude)
}else{
// command is raw
x = tell_command.lat;
y = tell_command.lng;
z = tell_command.alt;
}
// TODO - Need to add translation for many of the commands > MAV_CMD_NAV_LAST
// note XXX: documented x,y,z order does not match with gps raw
switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields
case MAV_CMD_NAV_LOITER_TURNS:
case MAV_CMD_NAV_TAKEOFF:
case MAV_CMD_CONDITION_CHANGE_ALT:
case MAV_CMD_DO_SET_HOME:
param1 = tell_command.p1;
break;
case MAV_CMD_NAV_LOITER_TIME:
param1 = tell_command.p1*10; // APM loiter time is in ten second increments
break;
case MAV_CMD_CONDITION_DELAY:
case MAV_CMD_CONDITION_DISTANCE:
param1 = tell_command.lat;
break;
case MAV_CMD_DO_JUMP:
param2 = tell_command.lat;
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_CHANGE_SPEED:
param3 = tell_command.lat;
param2 = tell_command.alt;
param1 = tell_command.p1;
break;
case MAV_CMD_DO_SET_PARAMETER:
case MAV_CMD_DO_SET_RELAY:
case MAV_CMD_DO_SET_SERVO:
param2 = tell_command.alt;
param1 = tell_command.p1;
break;
}
mavlink_msg_waypoint_send(chan,msg->sysid,
msg->compid,packet.seq,frame,command,current,autocontinue,
msg->compid,packet.seq,frame,tell_command.id,current,autocontinue,
param1,param2,param3,param4,x,y,z);
// update last waypoint comm stamp
@ -441,9 +479,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
}
g.waypoint_total.set_and_save(packet.count - 1);
global_data.waypoint_timelast_receive = millis();
global_data.waypoint_receiving = true;
global_data.waypoint_sending = false;
global_data.waypoint_request_i = 0;
global_data.waypoint_receiving = true;
global_data.waypoint_sending = false;
global_data.waypoint_request_i = 0;
break;
}
@ -469,9 +507,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
switch (packet.frame)
{
case MAV_FRAME_GLOBAL:
case MAV_FRAME_MISSION:
{
tell_command.p1 = packet.param1; // in as byte no conversion
//tell_command.p1 = packet.param1; // in as byte no conversion
tell_command.lat = 1.0e7*packet.x; // in as DD converted to * t7
tell_command.lng = 1.0e7*packet.y; // in as DD converted to * t7
tell_command.alt = packet.z*1.0e2; // in as m converted to cm
@ -481,31 +518,57 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAV_FRAME_LOCAL: // local (relative to home position)
{
tell_command.lat = 1.0e7*ToDeg(packet.x/
(radius_of_earth*cos(ToRad(home.lat/1.0e7)))) + home.lng;
tell_command.lng = 1.0e7*ToDeg(packet.y/radius_of_earth) + home.lat;
(radius_of_earth*cos(ToRad(home.lat/1.0e7)))) + home.lat;
tell_command.lng = 1.0e7*ToDeg(packet.y/radius_of_earth) + home.lng;
tell_command.alt = -packet.z*1.0e2 + home.alt;
break;
}
case MAV_FRAME_MISSION:
{
if(tell_command.id >= MAV_CMD_NAV_LAST){
// these are raw values
tell_command.p1 = packet.param1; // in as byte no conversion
tell_command.lat = packet.x; // in as long no conversion
tell_command.lng = packet.y; // in as long no conversion
tell_command.alt = packet.z; // in as int no conversion
}else{
tell_command.p1 = packet.param1; // in as byte no conversion
tell_command.lat = 1.0e7*packet.x; // in as DD converted to * t7
tell_command.lng = 1.0e7*packet.y; // in as DD converted to * t7
tell_command.alt = packet.z*1.0e2; // in as m converted to cm
}
break;
}
}
switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields
case MAV_CMD_NAV_LOITER_TURNS:
case MAV_CMD_NAV_TAKEOFF:
case MAV_CMD_DO_SET_HOME:
tell_command.p1 = packet.param1;
break;
case MAV_CMD_CONDITION_CHANGE_ALT:
tell_command.p1 = packet.param1 * 100;
break;
case MAV_CMD_NAV_LOITER_TIME:
tell_command.p1 = packet.param1 / 10; // APM loiter time is in ten second increments
break;
case MAV_CMD_CONDITION_DELAY:
case MAV_CMD_CONDITION_DISTANCE:
tell_command.lat = packet.param1;
break;
case MAV_CMD_DO_JUMP:
tell_command.lat = packet.param2;
tell_command.p1 = packet.param1;
break;
case MAV_CMD_DO_REPEAT_SERVO:
tell_command.lng = packet.param4;
case MAV_CMD_DO_REPEAT_RELAY:
case MAV_CMD_DO_CHANGE_SPEED:
tell_command.lat = packet.param3;
tell_command.alt = packet.param2;
tell_command.p1 = packet.param1;
break;
case MAV_CMD_DO_SET_PARAMETER:
case MAV_CMD_DO_SET_RELAY:
case MAV_CMD_DO_SET_SERVO:
tell_command.alt = packet.param2;
tell_command.p1 = packet.param1;
break;
}
// save waypoint - and prevent re-setting home
if (packet.seq != 0)
set_wp_with_index(tell_command, packet.seq);
@ -578,11 +641,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// Report back new value
mavlink_msg_param_value_send(
chan,
(int8_t *)key,
packet.param_value,
_count_parameters(),
-1); // XXX we don't actually know what its index is...
chan,
(int8_t *)key,
packet.param_value,
_count_parameters(),
-1); // XXX we don't actually know what its index is...
break;
} // end case
@ -782,22 +845,22 @@ GCS_MAVLINK::_queued_send()
}
// this is called at 50hz, count runs to prevent flooding serialport and delayed to allow eeprom write
mavdelay++;
mavdelay++;
// request waypoints one by one
// XXX note that this is pan-interface
if (global_data.waypoint_receiving &&
(global_data.requested_interface == chan) &&
global_data.waypoint_request_i <= (g.waypoint_total && mavdelay > 15)) // limits to 3.33 hz
{
mavlink_msg_waypoint_request_send(
chan,
global_data.waypoint_dest_sysid,
global_data.waypoint_dest_compid,
global_data.waypoint_request_i);
if (global_data.waypoint_receiving &&
(global_data.requested_interface == chan) &&
global_data.waypoint_request_i <= (g.waypoint_total && mavdelay > 15)){ // limits to 3.33 hz
mavdelay = 0;
}
mavlink_msg_waypoint_request_send(
chan,
global_data.waypoint_dest_sysid,
global_data.waypoint_dest_compid,
global_data.waypoint_request_i);
mavdelay = 0;
}
}
#endif

50
ArduCopterMega/Mavlink_Common.h
View File

@ -8,6 +8,8 @@
uint16_t system_type = MAV_FIXED_WING;
byte mavdelay = 0;
// what does this do?
static uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid)
{
if (sysid != mavlink_system.sysid){
@ -17,7 +19,7 @@ static uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid)
gcs.send_text(SEVERITY_LOW,"component id mismatch");
return 0; // XXX currently not receiving correct compid from gcs
} else {
}else{
return 0; // no error
}
}
@ -42,38 +44,31 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
switch(control_mode) {
case ACRO:
mode = MAV_MODE_MANUAL;
mode = MAV_MODE_MANUAL;
break;
case STABILIZE:
mode = MAV_MODE_GUIDED;
mode = MAV_MODE_GUIDED;
break;
case FBW:
mode = MAV_MODE_TEST1;
mode = MAV_MODE_TEST1;
break;
case ALT_HOLD:
mode = MAV_MODE_TEST2;
mode = MAV_MODE_TEST2;
break;
case LOITER:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_HOLD;
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_HOLD;
break;
case AUTO:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_WAYPOINT;
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_WAYPOINT;
break;
case RTL:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_RETURNING;
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_RETURNING;
break;
//case TAKEOFF:
// mode = MAV_MODE_AUTO;
// nav_mode = MAV_NAV_LIFTOFF;
// break;
//case LAND:
// mode = MAV_MODE_AUTO;
// nav_mode = MAV_NAV_LANDING;
// break;
}
uint8_t status = MAV_STATE_ACTIVE;
uint8_t motor_block = false;
@ -112,9 +107,9 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
current_loc.lat,
current_loc.lng,
current_loc.alt * 10,
g_gps->ground_speed / 1.0e2 * rot.a.x,
g_gps->ground_speed / 1.0e2 * rot.b.x,
g_gps->ground_speed / 1.0e2 * rot.c.x);
g_gps->ground_speed * rot.a.x,
g_gps->ground_speed * rot.b.x,
g_gps->ground_speed * rot.c.x);
break;
}
@ -160,7 +155,11 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
g.rc_2.norm_output(),
g.rc_3.norm_output(),
g.rc_4.norm_output(),
0,0,0,0,rssi);
0,
0,
0,
0,
rssi);
break;
}
@ -189,7 +188,10 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
motor_out[1],
motor_out[2],
motor_out[3],
0, 0, 0, 0);
0,
0,
0,
0);
break;
}

50
ArduCopterMega/command description.txt
View File

@ -23,24 +23,36 @@ Commands below MAV_CMD_NAV_LAST are commands that have a end criteria, eg "reach
***********************************
Command ID Name Parameter 1 Altitude Latitude Longitude
0x10 / 16 MAV_CMD_NAV_WAYPOINT - altitude lat lon
0x11 / 17 MAV_CMD_NAV_LOITER_UNLIM (indefinitely) - altitude lat lon
0x11 / 17 MAV_CMD_NAV_LOITER_UNLIM (indefinitely) altitude lat lon
0x12 / 18 MAV_CMD_NAV_LOITER_TURNS turns altitude lat lon
0x13 / 19 MAV_CMD_NAV_LOITER_TIME time (seconds*10) altitude lat lon
0x14 / 20 MAV_CMD_NAV_RETURN_TO_LAUNCH - altitude lat lon
0x15 / 21 MAV_CMD_NAV_LAND - altitude lat lon
0x16 / 22 MAV_CMD_NAV_TAKEOFF takeoff pitch altitude - -
NOTE: for command 0x16 the value takeoff pitch specifies the minimum pitch for the case with airspeed sensor and the target pitch for the case without.
0x17 / 23 MAV_CMD_NAV_TARGET - altitude lat lon
***********************************
May Commands - these commands are optional to finish
Command ID Name Parameter 1 Parameter 2 Parameter 3 Parameter 4
0x70 / 112 MAV_CMD_CONDITION_DELAY - - time (milliseconds) -
0x71 / 113 MAV_CMD_CONDITION_CHANGE_ALT rate (cm/sec) alt (finish) - -
0x72 / 114 MAV_CMD_NAV_LAND_OPTIONS (NOT CURRENTLY IN MAVLINK PROTOCOL)
0x23 MAV_CMD_CONDITION_ANGLE angle speed direction (-1,1) rel (1), abs (0)
0x70 / 112 MAV_CMD_CONDITION_DELAY - - time (seconds) -
0x71 / 113 MAV_CMD_CONDITION_CHANGE_ALT rate (cm/sec) alt (finish) - -
Note: rate must be > 10 cm/sec due to integer math
MAV_CMD_NAV_LAND_OPTIONS (NOT CURRENTLY IN MAVLINK PROTOCOL OR IMPLEMENTED IN APM)
0x72 / 114 MAV_CMD_CONDITION_DISTANCE - - distance (meters) -
0x71 / 115 MAV_CMD_CONDITION_YAW angle speed direction (-1,1) rel (1), abs (0)
***********************************
Unexecuted commands > MAV_CMD_NAV_LAST are dropped when ready for the next command < MAV_CMD_NAV_LAST so plan/queue commands accordingly!
@ -50,12 +62,24 @@ reached, the unexecuted CMD_MAV_CONDITION and CMD_MAV_DO commands would be skipp
Now Commands - these commands are executed once until no more new now commands are available
Command ID Name Parameter 1 Parameter 2 Parameter 3 Parameter 4
0xB1 / 177 MAV_CMD_DO_JUMP index repeat count
0XB2 / 178 MAV_CMD_DO_CHANGE_SPEED TODO - Fill in from protocol
0xB3 / 179 MAV_CMD_DO_SET_HOME
0xB4 / 180 MAV_CMD_DO_SET_PARAMETER
0xB5 / 181 MAV_CMD_DO_SET_RELAY
0xB6 / 182 MAV_CMD_DO_REPEAT_RELAY
0xB7 / 183 MAV_CMD_DO_SET_SERVO
0xB8 / 184 MAV_CMD_DO_REPEAT_SERVO
0xB1 / 177 MAV_CMD_DO_JUMP index - repeat count -
Note: The repeat count must be greater than 1 for the command to execute. Use a repeat count of 1 if you intend a single use.
0XB2 / 178 MAV_CMD_DO_CHANGE_SPEED Speed type Speed (m/s) Throttle (Percent) -
(0=Airspeed, 1=Ground Speed) (-1 indicates no change)(-1 indicates no change)
0xB3 / 179 MAV_CMD_DO_SET_HOME Use current altitude lat lon
(1=use current location, 0=use specified location)
0xB4 / 180 MAV_CMD_DO_SET_PARAMETER Param number Param value (NOT CURRENTLY IMPLEMENTED IN APM)
0xB5 / 181 MAV_CMD_DO_SET_RELAY Relay number On/off (1/0) - -
0xB6 / 182 MAV_CMD_DO_REPEAT_RELAY Relay number Cycle count Cycle time (sec) -
Note: Max cycle time = 60 sec, A repeat relay or repeat servo command will cancel any current repeating event
0xB7 / 183 MAV_CMD_DO_SET_SERVO Servo number (5-8) On/off (1/0) - -
0xB6 / 184 MAV_CMD_DO_REPEAT_SERVO Servo number (5-8) Cycle count Cycle time (sec) -
Note: Max cycle time = 60 sec, A repeat relay or repeat servo command will cancel any current repeating event

26
ArduCopterMega/commands.pde
View File

@ -55,9 +55,7 @@ struct Location get_wp_with_index(int i)
// Add on home altitude if we are a nav command
if(temp.id < 0x70){
//if((flight_options_mask & WP_OPT_ALT_RELATIVE) == 0){
temp.alt += home.alt;
//}
temp.alt += home.alt;
}
return temp;
@ -68,19 +66,6 @@ struct Location get_wp_with_index(int i)
void set_wp_with_index(struct Location temp, int i)
{
i = constrain(i, 0, g.waypoint_total.get());
//if(i > 0 && temp.id < 50){
// temp.options & WP_OPT_LOCATION
// remove home altitude if we are a nav command
// temp.alt -= home.alt;
//}
// Store the location relatove to home
//if((flight_options_mask & WP_OPT_ALT_RELATIVE) == 0){
temp.alt -= home.alt;
//}
uint32_t mem = WP_START_BYTE + (i * WP_SIZE);
eeprom_write_byte((uint8_t *) mem, temp.id);
@ -130,9 +115,11 @@ long read_alt_to_hold()
//This function sets the waypoint and modes for Return to Launch
//********************************************************************************
Location get_LOITER_home_wp()
{
//so we know where we are navigating from
next_WP = current_loc;
// read home position
struct Location temp = get_wp_with_index(0);
temp.id = MAV_CMD_NAV_LOITER_UNLIM;
@ -209,11 +196,6 @@ void init_home()
Serial.printf("gps alt: %ld", home.alt);
// ground altitude in centimeters for pressure alt calculations
// ------------------------------------------------------------
g.ground_pressure.save();
// Save Home to EEPROM
// -------------------
set_wp_with_index(home, 0);

418
ArduCopterMega/commands_logic.pde
View File

@ -3,8 +3,7 @@
/********************************************************************************/
// Command Event Handlers
/********************************************************************************/
void
handle_process_must()
void handle_process_must()
{
// reset navigation integrators
// -------------------------
@ -24,19 +23,37 @@ handle_process_must()
do_land();
break;
case MAV_CMD_NAV_LOITER_UNLIM: // Loiter indefinitely
do_loiter_unlimited();
break;
case MAV_CMD_NAV_LOITER_TURNS: // Loiter N Times
//do_loiter_turns();
break;
case MAV_CMD_NAV_LOITER_TIME:
do_loiter_time();
break;
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
do_RTL();
break;
default:
break;
}
}
void
handle_process_may()
void handle_process_may()
{
switch(next_command.id){
case MAV_CMD_CONDITION_DELAY:
do_delay();
do_wait_delay();
break;
case MAV_CMD_CONDITION_DISTANCE:
do_within_distance();
break;
case MAV_CMD_CONDITION_CHANGE_ALT:
@ -52,16 +69,20 @@ handle_process_may()
}
}
void
handle_process_now()
void handle_process_now()
{
switch(next_command.id){
case MAV_CMD_DO_SET_HOME:
init_home();
case MAV_CMD_DO_JUMP:
do_jump();
break;
case MAV_CMD_DO_REPEAT_SERVO:
new_event(&next_command);
case MAV_CMD_DO_CHANGE_SPEED:
//do_change_speed();
break;
case MAV_CMD_DO_SET_HOME:
do_set_home();
break;
case MAV_CMD_DO_SET_SERVO:
@ -71,34 +92,39 @@ handle_process_now()
case MAV_CMD_DO_SET_RELAY:
do_set_relay();
break;
case MAV_CMD_DO_REPEAT_SERVO:
do_repeat_servo();
break;
case MAV_CMD_DO_REPEAT_RELAY:
do_repeat_relay();
break;
}
}
void
handle_no_commands()
void handle_no_commands()
{
if (command_must_ID)
return;
switch (control_mode){
//case GCS_AUTO:
// set_mode(LOITER);
default:
set_mode(RTL);
//next_command = get_LOITER_home_wp();
//SendDebug("MSG <load_next_command> Preload RTL cmd id: ");
//SendDebugln(next_command.id,DEC);
break;
}
}
/********************************************************************************/
// Verify command Handlers
/********************************************************************************/
bool verify_must()
{
switch(command_must_ID) {
case MAV_CMD_NAV_TAKEOFF: // Takeoff!
case MAV_CMD_NAV_TAKEOFF:
return verify_takeoff();
break;
@ -106,10 +132,22 @@ bool verify_must()
return verify_land();
break;
case MAV_CMD_NAV_WAYPOINT: // reach a waypoint
case MAV_CMD_NAV_WAYPOINT:
return verify_nav_wp();
break;
case MAV_CMD_NAV_LOITER_UNLIM:
return false;
break;
case MAV_CMD_NAV_LOITER_TURNS:
return true;
break;
case MAV_CMD_NAV_LOITER_TIME:
return verify_loiter_time();
break;
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
return verify_RTL();
break;
@ -125,32 +163,54 @@ bool verify_may()
{
switch(command_may_ID) {
case MAV_CMD_CONDITION_YAW:
return verify_yaw();
case MAV_CMD_CONDITION_DELAY:
return verify_wait_delay();
break;
case MAV_CMD_CONDITION_DELAY:
return verify_delay();
case MAV_CMD_CONDITION_DISTANCE:
return verify_within_distance();
break;
case MAV_CMD_CONDITION_CHANGE_ALT:
return verify_change_alt();
break;
case MAV_CMD_CONDITION_YAW:
return verify_yaw();
break;
default:
//gcs.send_text(SEVERITY_HIGH,"<verify_must: default> No current May commands");
return false;
break;
}
}
/********************************************************************************/
// Must command implementations
// Nav (Must) commands
/********************************************************************************/
void
do_takeoff()
void do_RTL(void)
{
control_mode = LOITER;
Location temp = home;
temp.alt = read_alt_to_hold();
//so we know where we are navigating from
next_WP = current_loc;
// Loads WP from Memory
// --------------------
set_next_WP(&temp);
// output control mode to the ground station
gcs.send_message(MSG_HEARTBEAT);
if (g.log_bitmask & MASK_LOG_MODE)
Log_Write_Mode(control_mode);
}
void do_takeoff()
{
Location temp = current_loc;
temp.alt = next_command.alt;
@ -159,46 +219,12 @@ do_takeoff()
set_next_WP(&temp);
}
bool
verify_takeoff()
{
if (current_loc.alt > next_WP.alt){
takeoff_complete = true;
return true;
}else{
return false;
}
}
void
do_nav_wp()
void do_nav_wp()
{
set_next_WP(&next_command);
}
bool
verify_nav_wp()
{
update_crosstrack();
if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) {
//SendDebug("MSG <verify_must: MAV_CMD_NAV_WAYPOINT> REACHED_WAYPOINT #");
//SendDebugln(command_must_index,DEC);
char message[30];
sprintf(message,"Reached Waypoint #%i",command_must_index);
gcs.send_text(SEVERITY_LOW,message);
return true;
}
// add in a more complex case
// Doug to do
if(loiter_sum > 300){
gcs.send_text(SEVERITY_MEDIUM,"Missed WP");
return true;
}
return false;
}
void
do_land()
void do_land()
{
land_complete = false; // set flag to use g_gps ground course during TO. IMU will be doing yaw drift correction
velocity_land = 1000;
@ -209,39 +235,89 @@ do_land()
set_next_WP(&temp);
}
bool
verify_land()
void do_loiter_unlimited()
{
update_crosstrack();
set_next_WP(&next_command);
}
velocity_land = ((old_alt - current_loc.alt) *.05) + (velocity_land * .95);
void do_loiter_turns()
{
set_next_WP(&next_command);
loiter_total = next_command.p1 * 360;
}
void do_loiter_time()
{
set_next_WP(&next_command);
loiter_time = millis();
loiter_time_max = next_command.p1; // units are (seconds * 10)
}
/********************************************************************************/
// Verify Nav (Must) commands
/********************************************************************************/
bool verify_takeoff()
{
if (current_loc.alt > next_WP.alt){
takeoff_complete = true;
return true;
}else{
return false;
}
}
bool verify_land()
{
// XXX not sure
velocity_land = ((old_alt - current_loc.alt) *.2) + (velocity_land * .8);
old_alt = current_loc.alt;
if(velocity_land == 0){
land_complete = true;
if((current_loc.alt < home.alt + 100) && velocity_land == 0){
land_complete = true;
return true;
}
update_crosstrack();
return false;
}
// add a new command at end of command set to RTL.
void
do_RTL()
bool verify_nav_wp()
{
Location temp = home;
temp.alt = read_alt_to_hold();
update_crosstrack();
if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) {
//SendDebug("MSG <verify_must: MAV_CMD_NAV_WAYPOINT> REACHED_WAYPOINT #");
//SendDebugln(command_must_index,DEC);
char message[30];
sprintf(message,"Reached Waypoint #%i",command_must_index);
gcs.send_text(SEVERITY_LOW,message);
return true;
}
//so we know where we are navigating from
next_WP = current_loc;
// Loads WP from Memory
// --------------------
set_next_WP(&temp);
// Have we passed the WP?
if(loiter_sum > 90){
gcs.send_text(SEVERITY_MEDIUM,"Missed WP");
return true;
}
return false;
}
bool
verify_RTL()
bool verify_loiter_unlim()
{
return false;
}
bool verify_loiter_time()
{
if ((millis() - loiter_time) > (long)loiter_time_max * 10000l) { // scale loiter_time_max from (sec*10) to milliseconds
gcs.send_text(SEVERITY_LOW,"<verify_must: MAV_CMD_NAV_LOITER_TIME> LOITER time complete ");
return true;
}
return false;
}
bool verify_RTL()
{
if (wp_distance <= g.waypoint_radius) {
gcs.send_text(SEVERITY_LOW,"Reached home");
@ -252,11 +328,30 @@ verify_RTL()
}
/********************************************************************************/
// May command implementations
// Condition (May) commands
/********************************************************************************/
void
do_yaw()
void do_wait_delay()
{
condition_start = millis();
condition_value = next_command.lat * 1000; // convert to milliseconds
}
void do_change_alt()
{
Location temp = next_WP;
condition_start = current_loc.alt;
condition_value = next_command.alt + home.alt;
temp.alt = condition_value;
set_next_WP(&temp);
}
void do_within_distance()
{
condition_value = next_command.lat;
}
void do_yaw()
{
// p1: bearing
// alt: speed
@ -314,8 +409,47 @@ do_yaw()
//command_yaw_time = 9000/ 10 = 900° per second
}
bool
verify_yaw()
/********************************************************************************/
// Verify Condition (May) commands
/********************************************************************************/
bool verify_wait_delay()
{
if ((millis() - condition_start) > condition_value){
condition_value = 0;
return true;
}
return false;
}
bool verify_change_alt()
{
if (condition_start < next_WP.alt){
// we are going higer
if(current_loc.alt > next_WP.alt){
condition_value = 0;
return true;
}
}else{
// we are going lower
if(current_loc.alt < next_WP.alt){
condition_value = 0;
return true;
}
}
return false;
}
bool verify_within_distance()
{
if (wp_distance < condition_value){
condition_value = 0;
return true;
}
return false;
}
bool verify_yaw()
{
if((millis() - command_yaw_start_time) > command_yaw_time){
nav_yaw = command_yaw_end;
@ -329,63 +463,93 @@ verify_yaw()
}
}
void
do_delay()
/********************************************************************************/
// Do (Now) commands
/********************************************************************************/
void do_loiter_at_location()
{
delay_start = millis();
delay_timeout = next_command.lat;
next_WP = current_loc;
}
bool
verify_delay()
void do_jump()
{
if ((millis() - delay_start) > delay_timeout){
delay_timeout = 0;
return true;
}else{
return false;
struct Location temp;
if(next_command.lat > 0) {
command_must_index = 0;
command_may_index = 0;
temp = get_wp_with_index(g.waypoint_index);
temp.lat = next_command.lat - 1; // Decrement repeat counter
set_wp_with_index(temp, g.waypoint_index);
g.waypoint_index.set_and_save(next_command.p1 - 1);
}
}
void
do_change_alt()
void do_set_home()
{
Location temp = next_WP;
temp.alt = next_command.alt + home.alt;
set_next_WP(&temp);
}
bool
verify_change_alt()
{
if(abs(current_loc.alt - next_WP.alt) < 100){
return true;
}else{
return false;
if(next_command.p1 == 1) {
init_home();
} else {
home.id = MAV_CMD_NAV_WAYPOINT;
home.lng = next_command.lng; // Lon * 10**7
home.lat = next_command.lat; // Lat * 10**7
home.alt = max(next_command.alt, 0);
home_is_set = true;
}
}
/********************************************************************************/
// Now command implementations
/********************************************************************************/
void do_hold_position()
{
set_next_WP(&current_loc);
}
void do_set_servo()
{
APM_RC.OutputCh(next_command.p1, next_command.alt);
APM_RC.OutputCh(next_command.p1 - 1, next_command.alt);
}
void do_set_relay()
{
if (next_command.p1 == 0) {
if (next_command.p1 == 1) {
relay_on();
} else if (next_command.p1 == 1) {
} else if (next_command.p1 == 0) {
relay_off();
}else{
relay_toggle();
}
}
}
void do_repeat_servo()
{
event_id = next_command.p1 - 1;
if(next_command.p1 >= CH_5 + 1 && next_command.p1 <= CH_8 + 1) {
event_timer = 0;
event_delay = next_command.lng * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds)
event_repeat = next_command.lat * 2;
event_value = next_command.alt;
switch(next_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();
}
}
void do_repeat_relay()
{
event_id = RELAY_TOGGLE;
event_timer = 0;
event_delay = next_command.lat * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds)
event_repeat = next_command.alt * 2;
update_events();
}

89
ArduCopterMega/commands_process.pde
View File

@ -14,11 +14,7 @@ void update_commands(void)
if(control_mode == AUTO){
load_next_command_from_EEPROM();
process_next_command();
}else if(control_mode == GCS_AUTO){
/*if( there is a command recieved )
process_next_command();
*/
}
} // Other (eg GCS_Auto) modes may be implemented here
}
void verify_commands(void)
@ -29,6 +25,7 @@ void verify_commands(void)
if(verify_may()){
command_may_index = NO_COMMAND;
command_may_ID = NO_COMMAND;
}
}
@ -37,15 +34,7 @@ void load_next_command_from_EEPROM()
// fetch next command if the next command queue is empty
// -----------------------------------------------------
if(next_command.id == NO_COMMAND){
next_command = get_wp_with_index(g.waypoint_index + 1);
//if(next_command.id != NO_COMMAND){
//SendDebug("MSG <load_next_command> fetch found new cmd from list at index ");
//SendDebug((g.waypoint_index + 1),DEC);
//SendDebug(" with cmd id ");
//SendDebugln(next_command.id,DEC);
//}
}
// If the preload failed, return or just Loiter
@ -54,20 +43,18 @@ void load_next_command_from_EEPROM()
if(next_command.id == NO_COMMAND){
// we are out of commands!
gcs.send_text(SEVERITY_LOW,"out of commands!");
//SendDebug("MSG <load_next_command> out of commands, g.waypoint_index: ");
//SendDebugln(g.waypoint_index,DEC);
handle_no_commands();
}
}
void process_next_command()
{
// these are waypoint/Must commands
// these are Navigation/Must commands
// ---------------------------------
if (command_must_index == 0){ // no current command loaded
if (next_command.id < MAV_CMD_NAV_LAST ){
increment_WP_index();
//save_command_index(); // to Recover from in air Restart
//save_command_index(); // TO DO - fix - to Recover from in air Restart
command_must_index = g.waypoint_index;
//SendDebug("MSG <process_next_command> new command_must_id ");
@ -80,67 +67,60 @@ void process_next_command()
}
}
// these are May commands
// these are Condition/May commands
// ----------------------
if (command_may_index == 0){
if (next_command.id > MAV_CMD_NAV_LAST && next_command.id < MAV_CMD_CONDITION_LAST ){
increment_WP_index();// this command is from the command list in EEPROM
increment_WP_index(); // this command is from the command list in EEPROM
command_may_index = g.waypoint_index;
//Serial.print("new command_may_index ");
//Serial.println(command_may_index,DEC);
//SendDebug("MSG <process_next_command> new command_may_id ");
//SendDebug(next_command.id,DEC);
//Serial.print("new command_may_index ");
//Serial.println(command_may_index,DEC);
if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Cmd(g.waypoint_index, &next_command);
process_may();
}
}
// these are do it now commands
// ---------------------------
if (next_command.id > MAV_CMD_CONDITION_LAST){
increment_WP_index();// this command is from the command list in EEPROM
if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Cmd(g.waypoint_index, &next_command);
process_now();
// these are Do/Now commands
// ---------------------------
if (next_command.id > MAV_CMD_CONDITION_LAST){
increment_WP_index(); // this command is from the command list in EEPROM
//SendDebug("MSG <process_next_command> new command_now_id ");
//SendDebug(next_command.id,DEC);
if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Cmd(g.waypoint_index, &next_command);
process_now();
}
}
}
/*
These functions implement the waypoint commands.
*/
/**************************************************/
// These functions implement the commands.
/**************************************************/
void process_must()
{
//SendDebug("process must index: ");
//SendDebugln(command_must_index,DEC);
gcs.send_text(SEVERITY_LOW,"New cmd: <process_must>");
gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
// clear May indexes
command_may_index = 0;
command_may_ID = 0;
command_may_index = NO_COMMAND;
command_may_ID = NO_COMMAND;
command_must_ID = next_command.id;
// loads the waypoint into Next_WP struct
// --------------------------------------
//set_next_WP(&next_command);
command_must_ID = next_command.id;
handle_process_must();
// invalidate command so a new one is loaded
// -----------------------------------------
handle_process_must();
next_command.id = NO_COMMAND;
next_command.id = NO_COMMAND;
}
void process_may()
{
//Serial.print("process_may cmd# ");
//Serial.println(g.waypoint_index,DEC);
command_may_ID = next_command.id;
gcs.send_text(SEVERITY_LOW,"<process_may> New may command loaded:");
gcs.send_text(SEVERITY_LOW,"<process_may>");
gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
command_may_ID = next_command.id;
handle_process_may();
// invalidate command so a new one is loaded
@ -150,16 +130,13 @@ void process_may()
void process_now()
{
const float t5 = 100000.0;
//Serial.print("process_now cmd# ");
//Serial.println(g.waypoint_index,DEC);
gcs.send_text(SEVERITY_LOW, "<process_now> New now command loaded: ");
gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
handle_process_now();
// invalidate command so a new one is loaded
// -----------------------------------------
next_command.id = NO_COMMAND;
gcs.send_text(SEVERITY_LOW, "<process_now>");
gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
}

6
ArduCopterMega/config.h
View File

@ -541,12 +541,6 @@
# define USE_CURRENT_ALT FALSE
#endif
#if USE_CURRENT_ALT == TRUE
# define CONFIG_OPTIONS 0
#else
# define CONFIG_OPTIONS HOLD_ALT_ABOVE_HOME
#endif
//////////////////////////////////////////////////////////////////////////////
// Developer Items
//

24
ArduCopterMega/defines.h
View File

@ -12,6 +12,7 @@
#define MAX_SERVO_OUTPUT 2700
// active altitude sensor
// ----------------------
#define SONAR 0
#define BARO 1
@ -30,7 +31,7 @@
#define ToDeg(x) (x*57.2957795131) // *180/pi
#define DEBUG 0
#define LOITER_RANGE 30 // for calculating power outside of loiter radius
#define LOITER_RANGE 60 // for calculating power outside of loiter radius
#define T6 1000000
#define T7 10000000
@ -93,25 +94,16 @@
#define NUM_MODES 8
#define WP_OPT_ALT_RELATIVE (1<<0)
#define WP_OPT_ALT_CHANGE (1<<1)
#define WP_OPT_YAW (1<<2)
// ..
#define WP_OPT_CMD_WAIT (1<<7)
// Commands - Note that APM (1<<9)now uses a subset of the MAVLink protocol commands. See enum MAV_CMD in the GCS_Mavlink library
// Commands - Note that APM now uses a subset of the MAVLink protocol commands. See enum MAV_CMD in the GCS_Mavlink library
#define CMD_BLANK 0 // there is no command stored in the mem location requested
#define NO_COMMAND 0
//repeating events
#define NO_REPEAT 0
#define CH_4_TOGGLE 1
#define CH_5_TOGGLE 2
#define CH_6_TOGGLE 3
#define CH_7_TOGGLE 4
#define CH_5_TOGGLE 1
#define CH_6_TOGGLE 2
#define CH_7_TOGGLE 3
#define CH_8_TOGGLE 4
#define RELAY_TOGGLE 5
#define STOP_REPEAT 10
@ -261,4 +253,4 @@
#define EEPROM_MAX_ADDR 4096
// parameters get the first 1KiB of EEPROM, remainder is for waypoints
#define WP_START_BYTE 0x400 // where in memory home WP is stored + all other WP
#define WP_SIZE 15
#define WP_SIZE 14

198
ArduCopterMega/events.pde
View File

@ -4,20 +4,42 @@
This event will be called when the failsafe changes
boolean failsafe reflects the current state
*/
void failsafe_event()
void failsafe_on_event()
{
if (failsafe == true){
// This is how to handle a failsafe.
switch(control_mode)
{
// This is how to handle a failsafe.
switch(control_mode)
{
}
}else{
reset_I();
}
}
void failsafe_off_event()
{
/*
if (g.throttle_fs_action == 2){
// We're back in radio contact
// return to AP
// ---------------------------
// re-read the switch so we can return to our preferred mode
// --------------------------------------------------------
reset_control_switch();
// Reset control integrators
// ---------------------
reset_I();
}else if (g.throttle_fs_action == 1){
// We're back in radio contact
// return to Home
// we should already be in RTL and throttle set to cruise
// ------------------------------------------------------
set_mode(RTL);
g.throttle_cruise = THROTTLE_CRUISE;
}
*/
}
void low_battery_event(void)
{
gcs.send_text(SEVERITY_HIGH,"Low Battery!");
@ -26,104 +48,29 @@ void low_battery_event(void)
}
/*
4 simultaneous events
int event_original - original time in seconds
int event_countdown - count down to zero
byte event_countdown - ID for what to do
byte event_countdown - how many times to loop, 0 = indefinite
byte event_value - specific information for an event like PWM value
byte counterEvent - undo the event if nescessary
count down each one
new event
undo_event
*/
void new_event(struct Location *cmd)
void update_events() // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY
{
SendDebug("New Event Loaded ");
SendDebugln(cmd->p1,DEC);
if(event_repeat == 0 || (millis() - event_timer) < event_delay)
return;
if(cmd->p1 == STOP_REPEAT){
SendDebugln("STOP repeat ");
event_id = NO_REPEAT;
event_timer = -1;
undo_event = 0;
event_value = 0;
event_delay = 0;
event_repeat = 0; // convert seconds to millis
event_undo_value = 0;
repeat_forever = 0;
}else{
// reset the event timer
event_timer = millis();
event_id = cmd->p1;
event_value = cmd->alt;
event_delay = cmd->lat;
event_repeat = cmd->lng; // convert seconds to millis
event_undo_value = 0;
repeat_forever = (event_repeat == 0) ? 1:0;
}
/*
Serial.print("event_id: ");
Serial.println(event_id,DEC);
Serial.print("event_value: ");
Serial.println(event_value,DEC);
Serial.print("event_delay: ");
Serial.println(event_delay,DEC);
Serial.print("event_repeat: ");
Serial.println(event_repeat,DEC);
Serial.print("event_undo_value: ");
Serial.println(event_undo_value,DEC);
Serial.print("repeat_forever: ");
Serial.println(repeat_forever,DEC);
Serial.print("Event_timer: ");
Serial.println(event_timer,DEC);
*/
perform_event();
}
void perform_event()
{
if (event_repeat > 0){
event_repeat --;
}
switch(event_id) {
case CH_4_TOGGLE:
event_undo_value = g.rc_5.radio_out;
APM_RC.OutputCh(CH_5, event_value); // send to Servos
undo_event = 2;
break;
case CH_5_TOGGLE:
event_undo_value = g.rc_6.radio_out;
APM_RC.OutputCh(CH_6, event_value); // send to Servos
undo_event = 2;
break;
case CH_6_TOGGLE:
event_undo_value = g.rc_7.radio_out;
APM_RC.OutputCh(CH_7, event_value); // send to Servos
undo_event = 2;
break;
case CH_7_TOGGLE:
event_undo_value = g.rc_8.radio_out;
APM_RC.OutputCh(CH_8, event_value); // send to Servos
undo_event = 2;
event_undo_value = 1;
break;
case RELAY_TOGGLE:
event_undo_value = PORTL & B00000100 ? 1:0;
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) {
APM_RC.OutputCh(event_id, event_value); // send to Servos
} else {
APM_RC.OutputCh(event_id, event_undo_value);
}
}
if (event_id == RELAY_TOGGLE) {
relay_toggle();
SendDebug("toggle relay ");
SendDebugln(PORTL,BIN);
undo_event = 2;
break;
}
}
}
@ -142,54 +89,3 @@ void relay_toggle()
PORTL ^= B00000100;
}
void update_events()
{
// repeating events
if(undo_event == 1){
perform_event_undo();
undo_event = 0;
}else if(undo_event > 1){
undo_event --;
}
if(event_timer == -1)
return;
if((millis() - event_timer) > event_delay){
perform_event();
if(event_repeat > 0 || repeat_forever == 1){
event_repeat--;
event_timer = millis();
}else{
event_timer = -1;
}
}
}
void perform_event_undo()
{
switch(event_id) {
case CH_4_TOGGLE:
APM_RC.OutputCh(CH_5, event_undo_value); // send to Servos
break;
case CH_5_TOGGLE:
APM_RC.OutputCh(CH_6, event_undo_value); // send to Servos
break;
case CH_6_TOGGLE:
APM_RC.OutputCh(CH_7, event_undo_value); // send to Servos
break;
case CH_7_TOGGLE:
APM_RC.OutputCh(CH_8, event_undo_value); // send to Servos
break;
case RELAY_TOGGLE:
relay_toggle();
SendDebug("untoggle relay ");
SendDebugln(PORTL,BIN);
break;
}
}

5
ArduCopterMega/global_data.h
View File

@ -17,11 +17,8 @@
//
struct global_struct
{
// parameters
uint16_t requested_interface; // store port to use
AP_Var *parameter_p; // parameter pointer
// waypoints
uint16_t requested_interface; // request port to use
uint16_t waypoint_request_i; // request index
uint16_t waypoint_dest_sysid; // where to send requests
uint16_t waypoint_dest_compid; // "

25
ArduCopterMega/system.pde
View File

@ -281,11 +281,11 @@ void set_mode(byte mode)
break;
case STABILIZE:
do_hold_position();
do_loiter_at_location();
break;
case ALT_HOLD:
do_hold_position();
do_loiter_at_location();
break;
case AUTO:
@ -293,7 +293,7 @@ void set_mode(byte mode)
break;
case LOITER:
do_hold_position();
do_loiter_at_location();
break;
case RTL:
@ -320,25 +320,16 @@ void set_failsafe(boolean mode)
failsafe = mode;
if (failsafe == false){
// We're back in radio contact
// ---------------------------
// re-read the switch so we can return to our preferred mode
reset_control_switch();
// Reset control integrators
// ---------------------
reset_I();
// We've regained radio contact
// ----------------------------
failsafe_off_event();
}else{
// We've lost radio contact
// ------------------------
// nothing to do right now
}
failsafe_on_event();
// Let the user know what's up so they can override the behavior
// -------------------------------------------------------------
failsafe_event();
}
}
}