diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index 87f0630e91..c625f0d191 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -5,130 +5,130 @@ /********************************************************************************/ static void process_nav_command() { - switch(command_nav_queue.id){ + switch(command_nav_queue.id) { - case MAV_CMD_NAV_TAKEOFF: // 22 - do_takeoff(); - break; + case MAV_CMD_NAV_TAKEOFF: // 22 + do_takeoff(); + break; - case MAV_CMD_NAV_WAYPOINT: // 16 Navigate to Waypoint - do_nav_wp(); - break; + case MAV_CMD_NAV_WAYPOINT: // 16 Navigate to Waypoint + do_nav_wp(); + break; - case MAV_CMD_NAV_LAND: // 21 LAND to Waypoint - yaw_mode = YAW_HOLD; - do_land(); - break; + case MAV_CMD_NAV_LAND: // 21 LAND to Waypoint + yaw_mode = YAW_HOLD; + do_land(); + break; - case MAV_CMD_NAV_LOITER_UNLIM: // 17 Loiter indefinitely - do_loiter_unlimited(); - break; + case MAV_CMD_NAV_LOITER_UNLIM: // 17 Loiter indefinitely + do_loiter_unlimited(); + break; - case MAV_CMD_NAV_LOITER_TURNS: //18 Loiter N Times - do_loiter_turns(); - break; + case MAV_CMD_NAV_LOITER_TURNS: //18 Loiter N Times + do_loiter_turns(); + break; - case MAV_CMD_NAV_LOITER_TIME: // 19 - do_loiter_time(); - break; + case MAV_CMD_NAV_LOITER_TIME: // 19 + do_loiter_time(); + break; - case MAV_CMD_NAV_RETURN_TO_LAUNCH: //20 - do_RTL(); - break; + case MAV_CMD_NAV_RETURN_TO_LAUNCH: //20 + do_RTL(); + break; - // point the copter and camera at a region of interest (ROI) - case MAV_CMD_NAV_ROI: // 80 - do_nav_roi(); - break; + // point the copter and camera at a region of interest (ROI) + case MAV_CMD_NAV_ROI: // 80 + do_nav_roi(); + break; - default: - break; - } + default: + break; + } } static void process_cond_command() { - switch(command_cond_queue.id){ + switch(command_cond_queue.id) { - case MAV_CMD_CONDITION_DELAY: // 112 - do_wait_delay(); - break; + case MAV_CMD_CONDITION_DELAY: // 112 + do_wait_delay(); + break; - case MAV_CMD_CONDITION_DISTANCE: // 114 - do_within_distance(); - break; + case MAV_CMD_CONDITION_DISTANCE: // 114 + do_within_distance(); + break; - case MAV_CMD_CONDITION_CHANGE_ALT: // 113 - do_change_alt(); - break; + case MAV_CMD_CONDITION_CHANGE_ALT: // 113 + do_change_alt(); + break; - case MAV_CMD_CONDITION_YAW: // 115 - do_yaw(); - break; + case MAV_CMD_CONDITION_YAW: // 115 + do_yaw(); + break; - default: - break; - } + default: + break; + } } static void process_now_command() { - switch(command_cond_queue.id){ + switch(command_cond_queue.id) { - case MAV_CMD_DO_JUMP: // 177 - do_jump(); - break; + case MAV_CMD_DO_JUMP: // 177 + do_jump(); + break; - case MAV_CMD_DO_CHANGE_SPEED: // 178 - do_change_speed(); - break; + case MAV_CMD_DO_CHANGE_SPEED: // 178 + do_change_speed(); + break; - case MAV_CMD_DO_SET_HOME: // 179 - do_set_home(); - break; + case MAV_CMD_DO_SET_HOME: // 179 + do_set_home(); + break; - case MAV_CMD_DO_SET_SERVO: // 183 - do_set_servo(); - break; + case MAV_CMD_DO_SET_SERVO: // 183 + do_set_servo(); + break; - case MAV_CMD_DO_SET_RELAY: // 181 - do_set_relay(); - break; + case MAV_CMD_DO_SET_RELAY: // 181 + do_set_relay(); + break; - case MAV_CMD_DO_REPEAT_SERVO: // 184 - do_repeat_servo(); - break; + case MAV_CMD_DO_REPEAT_SERVO: // 184 + do_repeat_servo(); + break; - case MAV_CMD_DO_REPEAT_RELAY: // 182 - do_repeat_relay(); - break; + case MAV_CMD_DO_REPEAT_RELAY: // 182 + do_repeat_relay(); + break; #if CAMERA == ENABLED - case MAV_CMD_DO_CONTROL_VIDEO: // Control on-board camera capturing. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| - break; + case MAV_CMD_DO_CONTROL_VIDEO: // Control on-board camera capturing. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| + break; - case MAV_CMD_DO_DIGICAM_CONFIGURE: // Mission command to configure an on-board camera controller system. |Modes: P, TV, AV, M, Etc| Shutter speed: Divisor number for one second| Aperture: F stop number| ISO number e.g. 80, 100, 200, Etc| Exposure type enumerator| Command Identity| Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)| - break; + case MAV_CMD_DO_DIGICAM_CONFIGURE: // Mission command to configure an on-board camera controller system. |Modes: P, TV, AV, M, Etc| Shutter speed: Divisor number for one second| Aperture: F stop number| ISO number e.g. 80, 100, 200, Etc| Exposure type enumerator| Command Identity| Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)| + break; - case MAV_CMD_DO_DIGICAM_CONTROL: // Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty| - break; + case MAV_CMD_DO_DIGICAM_CONTROL: // Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty| + break; #endif #if MOUNT == ENABLED - case MAV_CMD_DO_MOUNT_CONFIGURE: // Mission command to configure a camera mount |Mount operation mode (see MAV_CONFIGURE_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty| - camera_mount.configure_cmd(); - break; + case MAV_CMD_DO_MOUNT_CONFIGURE: // Mission command to configure a camera mount |Mount operation mode (see MAV_CONFIGURE_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty| + camera_mount.configure_cmd(); + break; - case MAV_CMD_DO_MOUNT_CONTROL: // Mission command to control a camera mount |pitch(deg*100) or lat, depending on mount mode.| roll(deg*100) or lon depending on mount mode| yaw(deg*100) or alt (in cm) depending on mount mode| Empty| Empty| Empty| Empty| - camera_mount.control_cmd(); - break; + case MAV_CMD_DO_MOUNT_CONTROL: // Mission command to control a camera mount |pitch(deg*100) or lat, depending on mount mode.| roll(deg*100) or lon depending on mount mode| yaw(deg*100) or alt (in cm) depending on mount mode| Empty| Empty| Empty| Empty| + camera_mount.control_cmd(); + break; #endif - default: - // do nothing with unrecognized MAVLink messages - break; - } + default: + // do nothing with unrecognized MAVLink messages + break; + } } /********************************************************************************/ @@ -137,76 +137,76 @@ static void process_now_command() static bool verify_must() { - switch(command_nav_queue.id) { + switch(command_nav_queue.id) { - case MAV_CMD_NAV_TAKEOFF: - return verify_takeoff(); - break; + case MAV_CMD_NAV_TAKEOFF: + return verify_takeoff(); + break; - case MAV_CMD_NAV_WAYPOINT: - return verify_nav_wp(); - break; + case MAV_CMD_NAV_WAYPOINT: + return verify_nav_wp(); + break; - case MAV_CMD_NAV_LAND: - if(g.sonar_enabled == true){ - return verify_land_sonar(); - }else{ - return verify_land_baro(); - } - break; + case MAV_CMD_NAV_LAND: + if(g.sonar_enabled == true) { + return verify_land_sonar(); + }else{ + return verify_land_baro(); + } + break; - case MAV_CMD_NAV_LOITER_UNLIM: - return verify_loiter_unlimited(); - break; + case MAV_CMD_NAV_LOITER_UNLIM: + return verify_loiter_unlimited(); + break; - case MAV_CMD_NAV_LOITER_TURNS: - return verify_loiter_turns(); - break; + case MAV_CMD_NAV_LOITER_TURNS: + return verify_loiter_turns(); + break; - case MAV_CMD_NAV_LOITER_TIME: - return verify_loiter_time(); - break; + case MAV_CMD_NAV_LOITER_TIME: + return verify_loiter_time(); + break; - case MAV_CMD_NAV_RETURN_TO_LAUNCH: - return verify_RTL(); - break; + case MAV_CMD_NAV_RETURN_TO_LAUNCH: + return verify_RTL(); + break; - case MAV_CMD_NAV_ROI: // 80 - return verify_nav_roi(); - break; + case MAV_CMD_NAV_ROI: // 80 + return verify_nav_roi(); + break; - default: - //gcs_send_text_P(SEVERITY_HIGH,PSTR(" No current Must commands")); - return false; - break; - } + default: + //gcs_send_text_P(SEVERITY_HIGH,PSTR(" No current Must commands")); + return false; + break; + } } static bool verify_may() { - switch(command_cond_queue.id) { + switch(command_cond_queue.id) { - case MAV_CMD_CONDITION_DELAY: - return verify_wait_delay(); - break; + case MAV_CMD_CONDITION_DELAY: + return verify_wait_delay(); + break; - case MAV_CMD_CONDITION_DISTANCE: - return verify_within_distance(); - break; + 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_CHANGE_ALT: + return verify_change_alt(); + break; - case MAV_CMD_CONDITION_YAW: - return verify_yaw(); - break; + case MAV_CMD_CONDITION_YAW: + return verify_yaw(); + break; - default: - //gcs_send_text_P(SEVERITY_HIGH,PSTR(" No current May commands")); - return false; - break; - } + default: + //gcs_send_text_P(SEVERITY_HIGH,PSTR(" No current May commands")); + return false; + break; + } } /********************************************************************************/ @@ -215,25 +215,25 @@ static bool verify_may() static void do_RTL(void) { - // TODO: Altitude option from mission planner - Location temp = home; - temp.alt = get_RTL_alt(); + // TODO: Altitude option from mission planner + Location temp = home; + temp.alt = get_RTL_alt(); - //so we know where we are navigating from - // -------------------------------------- - next_WP = current_loc; + //so we know where we are navigating from + // -------------------------------------- + next_WP = current_loc; - // Loads WP from Memory - // -------------------- - set_next_WP(&temp); + // Loads WP from Memory + // -------------------- + set_next_WP(&temp); - // We want to come home and stop on a dime - slow_wp = true; + // We want to come home and stop on a dime + slow_wp = true; - // output control mode to the ground station - // ----------------------------------------- - gcs_send_message(MSG_HEARTBEAT); + // output control mode to the ground station + // ----------------------------------------- + gcs_send_message(MSG_HEARTBEAT); } /********************************************************************************/ @@ -242,114 +242,114 @@ static void do_RTL(void) static void do_takeoff() { - wp_control = LOITER_MODE; + wp_control = LOITER_MODE; - // Start with current location - Location temp = current_loc; + // Start with current location + Location temp = current_loc; - // alt is always relative - temp.alt = command_nav_queue.alt; + // alt is always relative + temp.alt = command_nav_queue.alt; - // prevent flips - reset_I_all(); + // prevent flips + reset_I_all(); - // Set our waypoint - set_next_WP(&temp); + // Set our waypoint + set_next_WP(&temp); } static void do_nav_wp() { - wp_control = WP_MODE; - slow_wp = false; + wp_control = WP_MODE; + slow_wp = false; - set_next_WP(&command_nav_queue); + set_next_WP(&command_nav_queue); - // this is our bitmask to verify we have met all conditions to move on - wp_verify_byte = 0; + // this is our bitmask to verify we have met all conditions to move on + wp_verify_byte = 0; - // this will be used to remember the time in millis after we reach or pass the WP. - loiter_time = 0; + // this will be used to remember the time in millis after we reach or pass the WP. + loiter_time = 0; - // this is the delay, stored in seconds and expanded to millis - loiter_time_max = command_nav_queue.p1 * 1000; + // this is the delay, stored in seconds and expanded to millis + loiter_time_max = command_nav_queue.p1 * 1000; - if((next_WP.options & WP_OPTION_ALT_REQUIRED) == false){ - wp_verify_byte |= NAV_ALTITUDE; - } + if((next_WP.options & WP_OPTION_ALT_REQUIRED) == false) { + wp_verify_byte |= NAV_ALTITUDE; + } } static void do_land() { - wp_control = LOITER_MODE; + wp_control = LOITER_MODE; - // just to make sure - land_complete = false; + // just to make sure + land_complete = false; - // landing boost lowers the main throttle to mimmick - // the effect of a user's hand - landing_boost = 0; + // landing boost lowers the main throttle to mimmick + // the effect of a user's hand + landing_boost = 0; - // A counter that goes up if our climb rate stalls out. - ground_detector = 0; + // A counter that goes up if our climb rate stalls out. + ground_detector = 0; - // hold at our current location - set_next_WP(¤t_loc); + // hold at our current location + set_next_WP(¤t_loc); - // Set a new target altitude - set_new_altitude(0); + // Set a new target altitude + set_new_altitude(0); } static void do_loiter_unlimited() { - wp_control = LOITER_MODE; + wp_control = LOITER_MODE; - //Serial.println("dloi "); - if(command_nav_queue.lat == 0){ - set_next_WP(¤t_loc); - wp_control = LOITER_MODE; - }else{ - set_next_WP(&command_nav_queue); - wp_control = WP_MODE; - } + //Serial.println("dloi "); + if(command_nav_queue.lat == 0) { + set_next_WP(¤t_loc); + wp_control = LOITER_MODE; + }else{ + set_next_WP(&command_nav_queue); + wp_control = WP_MODE; + } } static void do_loiter_turns() { - wp_control = CIRCLE_MODE; + wp_control = CIRCLE_MODE; - if(command_nav_queue.lat == 0){ - // allow user to specify just the altitude - if(command_nav_queue.alt > 0){ - current_loc.alt = command_nav_queue.alt; - } - set_next_WP(¤t_loc); - }else{ - set_next_WP(&command_nav_queue); - } + if(command_nav_queue.lat == 0) { + // allow user to specify just the altitude + if(command_nav_queue.alt > 0) { + current_loc.alt = command_nav_queue.alt; + } + set_next_WP(¤t_loc); + }else{ + set_next_WP(&command_nav_queue); + } - circle_WP = next_WP; + circle_WP = next_WP; - loiter_total = command_nav_queue.p1 * 360; - loiter_sum = 0; - old_target_bearing = target_bearing; + loiter_total = command_nav_queue.p1 * 360; + loiter_sum = 0; + old_target_bearing = target_bearing; - circle_angle = target_bearing + 18000; - circle_angle = wrap_360(circle_angle); - circle_angle *= RADX100; + circle_angle = target_bearing + 18000; + circle_angle = wrap_360(circle_angle); + circle_angle *= RADX100; } static void do_loiter_time() { - if(command_nav_queue.lat == 0){ - wp_control = LOITER_MODE; - loiter_time = millis(); - set_next_WP(¤t_loc); - }else{ - wp_control = WP_MODE; - set_next_WP(&command_nav_queue); - } + if(command_nav_queue.lat == 0) { + wp_control = LOITER_MODE; + loiter_time = millis(); + set_next_WP(¤t_loc); + }else{ + wp_control = WP_MODE; + set_next_WP(&command_nav_queue); + } - loiter_time_max = command_nav_queue.p1 * 1000; // units are (seconds) + loiter_time_max = command_nav_queue.p1 * 1000; // units are (seconds) } /********************************************************************************/ @@ -358,189 +358,189 @@ static void do_loiter_time() static bool verify_takeoff() { - // wait until we are ready! - if(g.rc_3.control_in == 0){ - return false; - } - // are we above our target altitude? - return (current_loc.alt > next_WP.alt); + // wait until we are ready! + if(g.rc_3.control_in == 0) { + return false; + } + // are we above our target altitude? + return (current_loc.alt > next_WP.alt); } // called at 10hz static bool verify_land_sonar() { - if(current_loc.alt > 300){ - wp_control = LOITER_MODE; - ground_detector = 0; - }else{ - // begin to pull down on the throttle - landing_boost++; - landing_boost = min(landing_boost, 40); - } + if(current_loc.alt > 300) { + wp_control = LOITER_MODE; + ground_detector = 0; + }else{ + // begin to pull down on the throttle + landing_boost++; + landing_boost = min(landing_boost, 40); + } - if(current_loc.alt < 200 ){ - wp_control = NO_NAV_MODE; - } + if(current_loc.alt < 200 ) { + wp_control = NO_NAV_MODE; + } - if(current_loc.alt < 150 ){ - // if we are low or don't seem to be decending much, increment ground detector - if(current_loc.alt < 80 || abs(climb_rate) < 20) { - landing_boost++; // reduce the throttle at twice the normal rate + if(current_loc.alt < 150 ) { + // if we are low or don't seem to be decending much, increment ground detector + if(current_loc.alt < 80 || abs(climb_rate) < 20) { + landing_boost++; // reduce the throttle at twice the normal rate - if(ground_detector < 30) { - ground_detector++; - }else if (ground_detector == 30){ - land_complete = true; - if(g.rc_3.control_in == 0){ - ground_detector++; - init_disarm_motors(); - } - return true; - } - } - } - return false; + if(ground_detector < 30) { + ground_detector++; + }else if (ground_detector == 30) { + land_complete = true; + if(g.rc_3.control_in == 0) { + ground_detector++; + init_disarm_motors(); + } + return true; + } + } + } + return false; } static bool verify_land_baro() { - if(current_loc.alt > 300){ - wp_control = LOITER_MODE; - ground_detector = 0; - }else{ - // begin to pull down on the throttle - landing_boost++; - landing_boost = min(landing_boost, 40); - } + if(current_loc.alt > 300) { + wp_control = LOITER_MODE; + ground_detector = 0; + }else{ + // begin to pull down on the throttle + landing_boost++; + landing_boost = min(landing_boost, 40); + } - if(current_loc.alt < 100 ){ - wp_control = NO_NAV_MODE; - } + if(current_loc.alt < 100 ) { + wp_control = NO_NAV_MODE; + } - if(current_loc.alt < 200 ){ - if(abs(climb_rate) < 40) { - landing_boost++; + if(current_loc.alt < 200 ) { + if(abs(climb_rate) < 40) { + landing_boost++; - if(ground_detector < 30) { - ground_detector++; - }else if (ground_detector == 30){ - land_complete = true; - if(g.rc_3.control_in == 0){ - ground_detector++; - init_disarm_motors(); - } - return true; - } - } - } - return false; + if(ground_detector < 30) { + ground_detector++; + }else if (ground_detector == 30) { + land_complete = true; + if(g.rc_3.control_in == 0) { + ground_detector++; + init_disarm_motors(); + } + return true; + } + } + } + return false; } static bool verify_nav_wp() { - // Altitude checking - if(next_WP.options & WP_OPTION_ALT_REQUIRED){ - // we desire a certain minimum altitude - if(alt_change_flag == REACHED_ALT){ + // Altitude checking + if(next_WP.options & WP_OPTION_ALT_REQUIRED) { + // we desire a certain minimum altitude + if(alt_change_flag == REACHED_ALT) { - // we have reached that altitude - wp_verify_byte |= NAV_ALTITUDE; - } - } + // we have reached that altitude + wp_verify_byte |= NAV_ALTITUDE; + } + } - // Did we pass the WP? // Distance checking - if((wp_distance <= (waypoint_radius * 100)) || check_missed_wp()){ + // Did we pass the WP? // Distance checking + if((wp_distance <= (waypoint_radius * 100)) || check_missed_wp()) { - // if we have a distance calc error, wp_distance may be less than 0 - if(wp_distance > 0){ - wp_verify_byte |= NAV_LOCATION; + // if we have a distance calc error, wp_distance may be less than 0 + if(wp_distance > 0) { + wp_verify_byte |= NAV_LOCATION; - if(loiter_time == 0){ - loiter_time = millis(); - } - } - } + if(loiter_time == 0) { + loiter_time = millis(); + } + } + } - // Hold at Waypoint checking, we cant move on until this is OK - if(wp_verify_byte & NAV_LOCATION){ - // we have reached our goal + // Hold at Waypoint checking, we cant move on until this is OK + if(wp_verify_byte & NAV_LOCATION) { + // we have reached our goal - // loiter at the WP - wp_control = LOITER_MODE; + // loiter at the WP + wp_control = LOITER_MODE; - if ((millis() - loiter_time) > loiter_time_max) { - wp_verify_byte |= NAV_DELAY; - //gcs_send_text_P(SEVERITY_LOW,PSTR("verify_must: LOITER time complete")); - //Serial.println("vlt done"); - } - } + if ((millis() - loiter_time) > loiter_time_max) { + wp_verify_byte |= NAV_DELAY; + //gcs_send_text_P(SEVERITY_LOW,PSTR("verify_must: LOITER time complete")); + //Serial.println("vlt done"); + } + } - if(wp_verify_byte >= 7){ - //if(wp_verify_byte & NAV_LOCATION){ - char message[30]; - sprintf(message,"Reached Command #%i",command_nav_index); - gcs_send_text(SEVERITY_LOW,message); - wp_verify_byte = 0; - copter_leds_nav_blink = 15; // Cause the CopterLEDs to blink three times to indicate waypoint reached - return true; - }else{ - return false; - } + if(wp_verify_byte >= 7) { + //if(wp_verify_byte & NAV_LOCATION){ + char message[30]; + sprintf(message,"Reached Command #%i",command_nav_index); + gcs_send_text(SEVERITY_LOW,message); + wp_verify_byte = 0; + copter_leds_nav_blink = 15; // Cause the CopterLEDs to blink three times to indicate waypoint reached + return true; + }else{ + return false; + } } static bool verify_loiter_unlimited() { - if(wp_control == WP_MODE && wp_distance <= (g.waypoint_radius * 100)){ - // switch to position hold - wp_control = LOITER_MODE; - } - return false; + if(wp_control == WP_MODE && wp_distance <= (g.waypoint_radius * 100)) { + // switch to position hold + wp_control = LOITER_MODE; + } + return false; } static bool verify_loiter_time() { - if(wp_control == LOITER_MODE){ - if ((millis() - loiter_time) > loiter_time_max) { - return true; - } - } - if(wp_control == WP_MODE && wp_distance <= (g.waypoint_radius * 100)){ - // reset our loiter time - loiter_time = millis(); - // switch to position hold - wp_control = LOITER_MODE; - } - return false; + if(wp_control == LOITER_MODE) { + if ((millis() - loiter_time) > loiter_time_max) { + return true; + } + } + if(wp_control == WP_MODE && wp_distance <= (g.waypoint_radius * 100)) { + // reset our loiter time + loiter_time = millis(); + // switch to position hold + wp_control = LOITER_MODE; + } + return false; } static bool verify_loiter_turns() { - //Serial.printf("loiter_sum: %d \n", loiter_sum); - // have we rotated around the center enough times? - // ----------------------------------------------- - if(abs(loiter_sum) > loiter_total) { - loiter_total = 0; - loiter_sum = 0; - //gcs_send_text_P(SEVERITY_LOW,PSTR("verify_must: LOITER orbits complete")); - // clear the command queue; - return true; - } - return false; + //Serial.printf("loiter_sum: %d \n", loiter_sum); + // have we rotated around the center enough times? + // ----------------------------------------------- + if(abs(loiter_sum) > loiter_total) { + loiter_total = 0; + loiter_sum = 0; + //gcs_send_text_P(SEVERITY_LOW,PSTR("verify_must: LOITER orbits complete")); + // clear the command queue; + return true; + } + return false; } static bool verify_RTL() { - wp_control = WP_MODE; + wp_control = WP_MODE; - // Did we pass the WP? // Distance checking - if((wp_distance <= (g.waypoint_radius * 100)) || check_missed_wp()){ - wp_control = LOITER_MODE; + // Did we pass the WP? // Distance checking + if((wp_distance <= (g.waypoint_radius * 100)) || check_missed_wp()) { + wp_control = LOITER_MODE; - //gcs_send_text_P(SEVERITY_LOW,PSTR("Reached home")); - return true; - }else{ - return false; - } + //gcs_send_text_P(SEVERITY_LOW,PSTR("Reached home")); + return true; + }else{ + return false; + } } /********************************************************************************/ @@ -549,79 +549,79 @@ static bool verify_RTL() static void do_wait_delay() { - //Serial.print("dwd "); - condition_start = millis(); - condition_value = command_cond_queue.lat * 1000; // convert to milliseconds - //Serial.println(condition_value,DEC); + //Serial.print("dwd "); + condition_start = millis(); + condition_value = command_cond_queue.lat * 1000; // convert to milliseconds + //Serial.println(condition_value,DEC); } static void do_change_alt() { - Location temp = next_WP; - condition_start = current_loc.alt; - //condition_value = command_cond_queue.alt; - temp.alt = command_cond_queue.alt; - set_next_WP(&temp); + Location temp = next_WP; + condition_start = current_loc.alt; + //condition_value = command_cond_queue.alt; + temp.alt = command_cond_queue.alt; + set_next_WP(&temp); } static void do_within_distance() { - condition_value = command_cond_queue.lat * 100; + condition_value = command_cond_queue.lat * 100; } static void do_yaw() { - //Serial.println("dyaw "); - yaw_tracking = MAV_ROI_NONE; + //Serial.println("dyaw "); + yaw_tracking = MAV_ROI_NONE; - // target angle in degrees - command_yaw_start = nav_yaw; // current position - command_yaw_start_time = millis(); + // target angle in degrees + command_yaw_start = nav_yaw; // current position + command_yaw_start_time = millis(); - command_yaw_dir = command_cond_queue.p1; // 1 = clockwise, 0 = counterclockwise - command_yaw_speed = command_cond_queue.lat * 100; // ms * 100 - command_yaw_relative = command_cond_queue.lng; // 1 = Relative, 0 = Absolute + command_yaw_dir = command_cond_queue.p1; // 1 = clockwise, 0 = counterclockwise + command_yaw_speed = command_cond_queue.lat * 100; // ms * 100 + command_yaw_relative = command_cond_queue.lng; // 1 = Relative, 0 = Absolute - // if unspecified turn at 30° per second - if(command_yaw_speed == 0) - command_yaw_speed = 3000; + // if unspecified turn at 30° per second + if(command_yaw_speed == 0) + command_yaw_speed = 3000; - // ensure direction is valid, if invalid default to counter clockwise - if(command_yaw_dir > 1) - command_yaw_dir = 0; // 0 = counter clockwise, 1 = clockwise + // ensure direction is valid, if invalid default to counter clockwise + if(command_yaw_dir > 1) + command_yaw_dir = 0; // 0 = counter clockwise, 1 = clockwise - if(command_yaw_relative == 1){ - // relative - command_yaw_delta = command_cond_queue.alt * 100; - if(command_yaw_dir == 0){ // 0 = counter clockwise, 1 = clockwise - command_yaw_end = command_yaw_start - command_yaw_delta; - }else{ - command_yaw_end = command_yaw_start + command_yaw_delta; - } - command_yaw_end = wrap_360(command_yaw_end); - }else{ - // absolute - command_yaw_end = command_cond_queue.alt * 100; + if(command_yaw_relative == 1) { + // relative + command_yaw_delta = command_cond_queue.alt * 100; + if(command_yaw_dir == 0) { // 0 = counter clockwise, 1 = clockwise + command_yaw_end = command_yaw_start - command_yaw_delta; + }else{ + command_yaw_end = command_yaw_start + command_yaw_delta; + } + command_yaw_end = wrap_360(command_yaw_end); + }else{ + // absolute + command_yaw_end = command_cond_queue.alt * 100; - // calculate the delta travel in deg * 100 - if(command_yaw_dir == 0){ // 0 = counter clockwise, 1 = clockwise - if(command_yaw_start > command_yaw_end){ - command_yaw_delta = command_yaw_start - command_yaw_end; - }else{ - command_yaw_delta = 36000 + (command_yaw_start - command_yaw_end); - } - }else{ - if(command_yaw_start >= command_yaw_end){ - command_yaw_delta = 36000 - (command_yaw_start - command_yaw_end); - }else{ - command_yaw_delta = command_yaw_end - command_yaw_start; - } - } - command_yaw_delta = wrap_360(command_yaw_delta); - } + // calculate the delta travel in deg * 100 + if(command_yaw_dir == 0) { // 0 = counter clockwise, 1 = clockwise + if(command_yaw_start > command_yaw_end) { + command_yaw_delta = command_yaw_start - command_yaw_end; + }else{ + command_yaw_delta = 36000 + (command_yaw_start - command_yaw_end); + } + }else{ + if(command_yaw_start >= command_yaw_end) { + command_yaw_delta = 36000 - (command_yaw_start - command_yaw_end); + }else{ + command_yaw_delta = command_yaw_end - command_yaw_start; + } + } + command_yaw_delta = wrap_360(command_yaw_delta); + } - // rate to turn deg per second - default is ten - command_yaw_time = (command_yaw_delta / command_yaw_speed) * 1000; + // rate to turn deg per second - default is ten + command_yaw_time = (command_yaw_delta / command_yaw_speed) * 1000; } @@ -631,74 +631,74 @@ static void do_yaw() static bool verify_wait_delay() { - //Serial.print("vwd"); - if ((unsigned)(millis() - condition_start) > (unsigned)condition_value){ - //Serial.println("y"); - condition_value = 0; - return true; - } - //Serial.println("n"); - return false; + //Serial.print("vwd"); + if ((unsigned)(millis() - condition_start) > (unsigned)condition_value) { + //Serial.println("y"); + condition_value = 0; + return true; + } + //Serial.println("n"); + return false; } static bool verify_change_alt() { - //Serial.printf("change_alt, ca:%d, na:%d\n", (int)current_loc.alt, (int)next_WP.alt); - if ((int32_t)condition_start < next_WP.alt){ - // we are going higer - if(current_loc.alt > next_WP.alt){ - return true; - } - }else{ - // we are going lower - if(current_loc.alt < next_WP.alt){ - return true; - } - } - return false; + //Serial.printf("change_alt, ca:%d, na:%d\n", (int)current_loc.alt, (int)next_WP.alt); + if ((int32_t)condition_start < next_WP.alt) { + // we are going higer + if(current_loc.alt > next_WP.alt) { + return true; + } + }else{ + // we are going lower + if(current_loc.alt < next_WP.alt) { + return true; + } + } + return false; } static bool verify_within_distance() { - //Serial.printf("cond dist :%d\n", (int)condition_value); - if (wp_distance < condition_value){ - condition_value = 0; - return true; - } - return false; + //Serial.printf("cond dist :%d\n", (int)condition_value); + if (wp_distance < condition_value) { + condition_value = 0; + return true; + } + return false; } static bool verify_yaw() { - //Serial.printf("vyaw %d\n", (int)(nav_yaw/100)); + //Serial.printf("vyaw %d\n", (int)(nav_yaw/100)); - if((millis() - command_yaw_start_time) > command_yaw_time){ - // time out - // make sure we hold at the final desired yaw angle - nav_yaw = command_yaw_end; - auto_yaw = nav_yaw; + if((millis() - command_yaw_start_time) > command_yaw_time) { + // time out + // make sure we hold at the final desired yaw angle + nav_yaw = command_yaw_end; + auto_yaw = nav_yaw; - // TO-DO: there's still a problem with Condition_yaw, it will do it two times(probably more) sometimes, if it hasn't reached the next waypoint yet. - // it should only do it one time so there should be code here to prevent another Condition_Yaw. + // TO-DO: there's still a problem with Condition_yaw, it will do it two times(probably more) sometimes, if it hasn't reached the next waypoint yet. + // it should only do it one time so there should be code here to prevent another Condition_Yaw. - //Serial.println("Y"); - return true; + //Serial.println("Y"); + return true; - }else{ - // else we need to be at a certain place - // power is a ratio of the time : .5 = half done - float power = (float)(millis() - command_yaw_start_time) / (float)command_yaw_time; + }else{ + // else we need to be at a certain place + // power is a ratio of the time : .5 = half done + float power = (float)(millis() - command_yaw_start_time) / (float)command_yaw_time; - if(command_yaw_dir == 0) { // 0 = counter clockwise, 1 = clockwise - nav_yaw = command_yaw_start - ((float)command_yaw_delta * power ); - }else{ - nav_yaw = command_yaw_start + ((float)command_yaw_delta * power ); - } - nav_yaw = wrap_360(nav_yaw); - auto_yaw = nav_yaw; - //Serial.printf("ny %ld\n",nav_yaw); - return false; - } + if(command_yaw_dir == 0) { // 0 = counter clockwise, 1 = clockwise + nav_yaw = command_yaw_start - ((float)command_yaw_delta * power ); + }else{ + nav_yaw = command_yaw_start + ((float)command_yaw_delta * power ); + } + nav_yaw = wrap_360(nav_yaw); + auto_yaw = nav_yaw; + //Serial.printf("ny %ld\n",nav_yaw); + return false; + } } // verify_nav_roi - verifies that actions required by MAV_CMD_NAV_ROI have completed @@ -709,28 +709,28 @@ static bool verify_yaw() static bool verify_nav_roi() { #if MOUNT == ENABLED - // check if mount type requires us to rotate the quad - if( camera_mount.get_mount_type() == AP_Mount::k_tilt_roll ) { - // ensure yaw has gotten to within 2 degrees of the target - if( labs(wrap_180(ahrs.yaw_sensor-auto_yaw)) <= 200 ) { - nav_yaw = auto_yaw; // ensure target yaw for YAW_HOLD is our desired yaw - return true; - }else{ - return false; - } - }else{ - // if no rotation required, assume the camera instruction was implemented immediately - return true; - } + // check if mount type requires us to rotate the quad + if( camera_mount.get_mount_type() == AP_Mount::k_tilt_roll ) { + // ensure yaw has gotten to within 2 degrees of the target + if( labs(wrap_180(ahrs.yaw_sensor-auto_yaw)) <= 200 ) { + nav_yaw = auto_yaw; // ensure target yaw for YAW_HOLD is our desired yaw + return true; + }else{ + return false; + } + }else{ + // if no rotation required, assume the camera instruction was implemented immediately + return true; + } #else - // if we have no camera mount simply check we've reached the desired yaw - // ensure yaw has gotten to within 2 degrees of the target - if( abs(wrap_180(ahrs.yaw_sensor-auto_yaw)) <= 200 ) { - nav_yaw = auto_yaw; // ensure target yaw for YAW_HOLD is our desired yaw - return true; - }else{ - return false; - } + // if we have no camera mount simply check we've reached the desired yaw + // ensure yaw has gotten to within 2 degrees of the target + if( abs(wrap_180(ahrs.yaw_sensor-auto_yaw)) <= 200 ) { + nav_yaw = auto_yaw; // ensure target yaw for YAW_HOLD is our desired yaw + return true; + }else{ + return false; + } #endif } @@ -740,121 +740,121 @@ static bool verify_nav_roi() static void do_change_speed() { - g.waypoint_speed_max = command_cond_queue.p1 * 100; + g.waypoint_speed_max = command_cond_queue.p1 * 100; } static void do_target_yaw() { - yaw_tracking = command_cond_queue.p1; + yaw_tracking = command_cond_queue.p1; - if(yaw_tracking == MAV_ROI_LOCATION){ - target_WP = command_cond_queue; - } + if(yaw_tracking == MAV_ROI_LOCATION) { + target_WP = command_cond_queue; + } } static void do_loiter_at_location() { - next_WP = current_loc; + next_WP = current_loc; } static void do_jump() { - // Used to track the state of the jump command in Mission scripting - // -10 is a value that means the register is unused - // when in use, it contains the current remaining jumps - static int8_t jump = -10; // used to track loops in jump command + // Used to track the state of the jump command in Mission scripting + // -10 is a value that means the register is unused + // when in use, it contains the current remaining jumps + static int8_t jump = -10; // used to track loops in jump command - //Serial.printf("do Jump: %d\n", jump); + //Serial.printf("do Jump: %d\n", jump); - if(jump == -10){ - //Serial.printf("Fresh Jump\n"); - // we use a locally stored index for jump - jump = command_cond_queue.lat; - } - //Serial.printf("Jumps left: %d\n",jump); + if(jump == -10) { + //Serial.printf("Fresh Jump\n"); + // we use a locally stored index for jump + jump = command_cond_queue.lat; + } + //Serial.printf("Jumps left: %d\n",jump); - if(jump > 0) { - //Serial.printf("Do Jump to %d\n",command_cond_queue.p1); - jump--; - change_command(command_cond_queue.p1); + if(jump > 0) { + //Serial.printf("Do Jump to %d\n",command_cond_queue.p1); + jump--; + change_command(command_cond_queue.p1); - } else if (jump == 0){ - //Serial.printf("Did last jump\n"); - // we're done, move along - jump = -11; + } else if (jump == 0) { + //Serial.printf("Did last jump\n"); + // we're done, move along + jump = -11; - } else if (jump == -1) { - //Serial.printf("jumpForever\n"); - // repeat forever - change_command(command_cond_queue.p1); - } + } else if (jump == -1) { + //Serial.printf("jumpForever\n"); + // repeat forever + change_command(command_cond_queue.p1); + } } static void do_set_home() { - if(command_cond_queue.p1 == 1) { - init_home(); - } else { - home.id = MAV_CMD_NAV_WAYPOINT; - home.lng = command_cond_queue.lng; // Lon * 10**7 - home.lat = command_cond_queue.lat; // Lat * 10**7 - home.alt = 0; - home_is_set = true; - } + if(command_cond_queue.p1 == 1) { + init_home(); + } else { + home.id = MAV_CMD_NAV_WAYPOINT; + home.lng = command_cond_queue.lng; // Lon * 10**7 + home.lat = command_cond_queue.lat; // Lat * 10**7 + home.alt = 0; + home_is_set = true; + } } static void do_set_servo() { - APM_RC.OutputCh(command_cond_queue.p1 - 1, command_cond_queue.alt); + APM_RC.OutputCh(command_cond_queue.p1 - 1, command_cond_queue.alt); } static void do_set_relay() { - if (command_cond_queue.p1 == 1) { - relay.on(); - } else if (command_cond_queue.p1 == 0) { - relay.off(); - }else{ - relay.toggle(); - } + if (command_cond_queue.p1 == 1) { + relay.on(); + } else if (command_cond_queue.p1 == 0) { + relay.off(); + }else{ + relay.toggle(); + } } static void do_repeat_servo() { - event_id = command_cond_queue.p1 - 1; + event_id = command_cond_queue.p1 - 1; - if(command_cond_queue.p1 >= CH_5 + 1 && command_cond_queue.p1 <= CH_8 + 1) { + if(command_cond_queue.p1 >= CH_5 + 1 && command_cond_queue.p1 <= CH_8 + 1) { - event_timer = 0; - event_value = command_cond_queue.alt; - event_repeat = command_cond_queue.lat * 2; - event_delay = command_cond_queue.lng * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds) + event_timer = 0; + event_value = command_cond_queue.alt; + event_repeat = command_cond_queue.lat * 2; + event_delay = command_cond_queue.lng * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds) - switch(command_cond_queue.p1) { - case CH_5: - event_undo_value = g.rc_5.radio_trim; - break; - case CH_6: - event_undo_value = g.rc_6.radio_trim; - break; - case CH_7: - event_undo_value = g.rc_7.radio_trim; - break; - case CH_8: - event_undo_value = g.rc_8.radio_trim; - break; - } - update_events(); - } + switch(command_cond_queue.p1) { + case CH_5: + event_undo_value = g.rc_5.radio_trim; + break; + case CH_6: + event_undo_value = g.rc_6.radio_trim; + break; + case CH_7: + event_undo_value = g.rc_7.radio_trim; + break; + case CH_8: + event_undo_value = g.rc_8.radio_trim; + break; + } + update_events(); + } } static void do_repeat_relay() { - event_id = RELAY_TOGGLE; - event_timer = 0; - event_delay = command_cond_queue.lat * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds) - event_repeat = command_cond_queue.alt * 2; - update_events(); + event_id = RELAY_TOGGLE; + event_timer = 0; + event_delay = command_cond_queue.lat * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds) + event_repeat = command_cond_queue.alt * 2; + update_events(); } // do_nav_roi - starts actions required by MAV_CMD_NAV_ROI @@ -866,25 +866,25 @@ static void do_nav_roi() { #if MOUNT == ENABLED - // check if mount type requires us to rotate the quad - if( camera_mount.get_mount_type() == AP_Mount::k_tilt_roll ) { - yaw_tracking = MAV_ROI_LOCATION; - target_WP = command_nav_queue; - auto_yaw = get_bearing_cd(¤t_loc, &target_WP); - } - // send the command to the camera mount - camera_mount.set_roi_cmd(&command_nav_queue); + // check if mount type requires us to rotate the quad + if( camera_mount.get_mount_type() == AP_Mount::k_tilt_roll ) { + yaw_tracking = MAV_ROI_LOCATION; + target_WP = command_nav_queue; + auto_yaw = get_bearing_cd(¤t_loc, &target_WP); + } + // send the command to the camera mount + camera_mount.set_roi_cmd(&command_nav_queue); - // TO-DO: expand handling of the do_nav_roi to support all modes of the MAVLink. Currently we only handle mode 4 (see below) - // 0: do nothing - // 1: point at next waypoint - // 2: point at a waypoint taken from WP# parameter (2nd parameter?) - // 3: point at a location given by alt, lon, lat parameters - // 4: point at a target given a target id (can't be implmented) + // TO-DO: expand handling of the do_nav_roi to support all modes of the MAVLink. Currently we only handle mode 4 (see below) + // 0: do nothing + // 1: point at next waypoint + // 2: point at a waypoint taken from WP# parameter (2nd parameter?) + // 3: point at a location given by alt, lon, lat parameters + // 4: point at a target given a target id (can't be implmented) #else - // if we have no camera mount simply rotate the quad - yaw_tracking = MAV_ROI_LOCATION; - target_WP = command_nav_queue; - auto_yaw = get_bearing_cd(¤t_loc, &target_WP); + // if we have no camera mount simply rotate the quad + yaw_tracking = MAV_ROI_LOCATION; + target_WP = command_nav_queue; + auto_yaw = get_bearing_cd(¤t_loc, &target_WP); #endif }