diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 0f7afaf818..d497e241f7 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -738,8 +738,8 @@ static int16_t event_undo_value; //////////////////////////////////////////////////////////////////////////////// // Delay Mission Scripting Command //////////////////////////////////////////////////////////////////////////////// -static int32_t condition_value; // used in condition commands (eg delay, change alt, etc.) -static int32_t condition_start; +static int32_t condition_value; // used in condition commands (eg delay, change alt, etc.) +static uint32_t condition_start; //////////////////////////////////////////////////////////////////////////////// diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index a480c2210c..a426f748cb 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -386,8 +386,6 @@ static bool verify_land() old_alt = 0; return false; } - //Serial.printf("N, %d\n", velocity_land); - //Serial.printf("N_alt, %ld\n", next_WP.alt); return false; } @@ -509,7 +507,7 @@ static void do_change_alt() { Location temp = next_WP; condition_start = current_loc.alt; - condition_value = command_cond_queue.alt; + //condition_value = command_cond_queue.alt; temp.alt = command_cond_queue.alt; set_next_WP(&temp); } @@ -582,9 +580,9 @@ static void do_yaw() static bool verify_wait_delay() { //Serial.print("vwd"); - if ((unsigned)(millis() - condition_start) > condition_value){ + if ((unsigned)(millis() - condition_start) > (unsigned)condition_value){ //Serial.println("y"); - condition_value = 0; + condition_value = 0; return true; } //Serial.println("n"); @@ -597,13 +595,11 @@ static 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; } } diff --git a/Tools/autotest/pysim/sim_multicopter.py b/Tools/autotest/pysim/sim_multicopter.py index 184713f1f5..a3cb744a05 100755 --- a/Tools/autotest/pysim/sim_multicopter.py +++ b/Tools/autotest/pysim/sim_multicopter.py @@ -168,10 +168,10 @@ while True: frame_count += 1 t = time.time() if t - lastt > 1.0: - print("%.2f fps zspeed=%.2f zaccel=%.2f h=%.1f a=%.1f yaw=%.1f yawrate=%.1f" % ( - frame_count/(t-lastt), - a.velocity.z, a.accel.z, a.position.z, a.altitude, - a.yaw, a.yaw_rate)) + #print("%.2f fps zspeed=%.2f zaccel=%.2f h=%.1f a=%.1f yaw=%.1f yawrate=%.1f" % ( + # frame_count/(t-lastt), + # a.velocity.z, a.accel.z, a.position.z, a.altitude, + # a.yaw, a.yaw_rate)) lastt = t frame_count = 0 frame_end = time.time() diff --git a/libraries/Desktop/support/sitl.cpp b/libraries/Desktop/support/sitl.cpp index a085662a2b..8e4d35c904 100644 --- a/libraries/Desktop/support/sitl.cpp +++ b/libraries/Desktop/support/sitl.cpp @@ -135,7 +135,7 @@ static void sitl_fdm_input(void) count++; if (millis() - last_report > 1000) { - printf("SIM %u FPS\n", count); + //printf("SIM %u FPS\n", count); count = 0; last_report = millis(); } diff --git a/libraries/Desktop/support/sitl_adc.cpp b/libraries/Desktop/support/sitl_adc.cpp index 7b241a9a21..8ea7de885d 100644 --- a/libraries/Desktop/support/sitl_adc.cpp +++ b/libraries/Desktop/support/sitl_adc.cpp @@ -111,11 +111,12 @@ void sitl_update_adc(float roll, float pitch, float yaw, (fabs(roll - dcm.roll_sensor/100.0) > 5.0 || fabs(pitch - dcm.pitch_sensor/100.0) > 5.0))) { last_report = tnow; - printf("roll=%5.1f / %5.1f pitch=%5.1f / %5.1f rr=%5.2f / %5.2f pr=%5.2f / %5.2f\n", + /*printf("roll=%5.1f / %5.1f pitch=%5.1f / %5.1f rr=%5.2f / %5.2f pr=%5.2f / %5.2f\n", roll, dcm.roll_sensor/100.0, pitch, dcm.pitch_sensor/100.0, rollRate, ToDeg(omega.x), pitchRate, ToDeg(omega.y)); + */ } }