diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index 15b6c30a82..e3ce6d8e66 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -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 // ------------------- diff --git a/ArduCopterMega/Attitude.pde b/ArduCopterMega/Attitude.pde index f22ba0ed35..33bae47e78 100644 --- a/ArduCopterMega/Attitude.pde +++ b/ArduCopterMega/Attitude.pde @@ -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° } } diff --git a/ArduCopterMega/GCS_Mavlink.pde b/ArduCopterMega/GCS_Mavlink.pde index bc196d0f4f..0c833f8fdc 100644 --- a/ArduCopterMega/GCS_Mavlink.pde +++ b/ArduCopterMega/GCS_Mavlink.pde @@ -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 diff --git a/ArduCopterMega/Mavlink_Common.h b/ArduCopterMega/Mavlink_Common.h index 889bcb4240..08f78912e3 100644 --- a/ArduCopterMega/Mavlink_Common.h +++ b/ArduCopterMega/Mavlink_Common.h @@ -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; } diff --git a/ArduCopterMega/command description.txt b/ArduCopterMega/command description.txt index d851988808..68b297f2cc 100644 --- a/ArduCopterMega/command description.txt +++ b/ArduCopterMega/command description.txt @@ -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 diff --git a/ArduCopterMega/commands.pde b/ArduCopterMega/commands.pde index 915bfaa722..7365939e43 100644 --- a/ArduCopterMega/commands.pde +++ b/ArduCopterMega/commands.pde @@ -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); diff --git a/ArduCopterMega/commands_logic.pde b/ArduCopterMega/commands_logic.pde index 650a7578ae..1a01eb859c 100644 --- a/ArduCopterMega/commands_logic.pde +++ b/ArduCopterMega/commands_logic.pde @@ -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 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," 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 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 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," 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(¤t_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(); +} \ No newline at end of file diff --git a/ArduCopterMega/commands_process.pde b/ArduCopterMega/commands_process.pde index 6f5d497a04..064b1db037 100644 --- a/ArduCopterMega/commands_process.pde +++ b/ArduCopterMega/commands_process.pde @@ -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 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 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 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 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 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: "); 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," New may command loaded:"); + gcs.send_text(SEVERITY_LOW,""); 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, " 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, ""); + gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index); } diff --git a/ArduCopterMega/config.h b/ArduCopterMega/config.h index f9ac12d0bc..bcda642387 100644 --- a/ArduCopterMega/config.h +++ b/ArduCopterMega/config.h @@ -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 // diff --git a/ArduCopterMega/defines.h b/ArduCopterMega/defines.h index 6e7477f9c3..de4fb29c71 100644 --- a/ArduCopterMega/defines.h +++ b/ArduCopterMega/defines.h @@ -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 diff --git a/ArduCopterMega/events.pde b/ArduCopterMega/events.pde index 1b54b74e0e..504ad380d1 100644 --- a/ArduCopterMega/events.pde +++ b/ArduCopterMega/events.pde @@ -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; - } -} diff --git a/ArduCopterMega/global_data.h b/ArduCopterMega/global_data.h index ef01589f8e..c1a7c383f6 100644 --- a/ArduCopterMega/global_data.h +++ b/ArduCopterMega/global_data.h @@ -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; // " diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde index 1e3fe0187f..8e20c0ea2b 100644 --- a/ArduCopterMega/system.pde +++ b/ArduCopterMega/system.pde @@ -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(); + } } }