diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c index 8b4b5c400f..8ed6db6642 100644 --- a/apps/ardrone_interface/ardrone_interface.c +++ b/apps/ardrone_interface/ardrone_interface.c @@ -141,7 +141,6 @@ static int ardrone_open_uart(char *uart_name, struct termios *uart_config_origin int uart; /* open uart */ - //printf("[ardrone_interface] UART is %s, baud rate is%d\n",uart_name,speed); uart = open(uart_name, O_RDWR | O_NOCTTY); /* Try to set baud rate */ @@ -238,15 +237,14 @@ int ardrone_interface_thread_main(int argc, char *argv[]) /* declare and safely initialize all structs */ struct vehicle_status_s state; - //memset(&state, 0, sizeof(state)); + memset(&state, 0, sizeof(state)); struct actuator_controls_s actuator_controls; - //memset(&actuator_controls, 0, sizeof(actuator_controls)); + memset(&actuator_controls, 0, sizeof(actuator_controls)); struct actuator_armed_s armed; armed.armed = false; /* subscribe to attitude, motor setpoints and system state */ int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); - int state_sub = orb_subscribe(ORB_ID(vehicle_status)); int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); @@ -328,11 +326,6 @@ int ardrone_interface_thread_main(int argc, char *argv[]) * if in failsafe */ if (armed.armed && !armed.lockdown) { - - - - //printf("AMO_BEF: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",actuator_controls.control[0], actuator_controls.control[1], actuator_controls.control[2], actuator_controls.control[3]); - ardrone_mixing_and_output(ardrone_write, &actuator_controls); } else { diff --git a/apps/ardrone_interface/ardrone_motor_control.c b/apps/ardrone_interface/ardrone_motor_control.c index e410d3a71a..c68a26df96 100644 --- a/apps/ardrone_interface/ardrone_motor_control.c +++ b/apps/ardrone_interface/ardrone_motor_control.c @@ -388,16 +388,12 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls if (motor_thrust <= min_thrust) { motor_thrust = min_thrust; output_band = 0.0f; - //printf("0 silent\n"); } else if (motor_thrust < startpoint_full_control && motor_thrust > min_thrust) { output_band = band_factor * (motor_thrust - min_thrust); - //printf("1 starting\n"); } else if (motor_thrust >= startpoint_full_control && motor_thrust < max_thrust - band_factor * startpoint_full_control) { output_band = band_factor * startpoint_full_control; - //printf("2 working\n"); } else if (motor_thrust >= max_thrust - band_factor * startpoint_full_control) { output_band = band_factor * (max_thrust - motor_thrust); - //printf("3 full\n"); } //add the yaw, nick and roll components to the basic thrust //TODO:this should be done by the mixer diff --git a/apps/commander/commander.c b/apps/commander/commander.c index a1c2a15d26..1c23c1f9d2 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -404,10 +404,6 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) /* announce and set new offset */ - // char offset_output[50]; - // sprintf(offset_output, "[commander] mag cal: %8.4f %8.4f %8.4f", (double)mag_offset[0], (double)mag_offset[1], (double)mag_offset[2]); - // mavlink_log_info(mavlink_fd, offset_output); - if (param_set(param_find("SENS_MAG_XOFF"), &(mag_offset[0]))) { fprintf(stderr, "[commander] Setting X mag offset failed!\n"); } diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index b00b6bc0ca..ebd9911a36 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -236,7 +236,6 @@ mc_thread_main(int argc, char *argv[]) /* set yaw rate */ rates_sp.yaw = manual.yaw; att_sp.thrust = manual.throttle; - //printf("Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",att_sp.roll_body, att_sp.pitch_body, att_sp.yaw_body, att_sp.thrust); att_sp.timestamp = hrt_absolute_time(); /* STEP 2: publish the result to the vehicle actuators */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); @@ -278,15 +277,10 @@ mc_thread_main(int argc, char *argv[]) /* run attitude controller */ if (state.flag_control_attitude_enabled && !state.flag_control_rates_enabled) { multirotor_control_attitude(&att_sp, &att, NULL, &actuators); -// printf("publish actuator\n"); - -// printf("MAC_PUB: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",actuators.control[0], actuators.control[1], actuators.control[2], actuators.control[3]); - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); } else if (state.flag_control_attitude_enabled && state.flag_control_rates_enabled) { multirotor_control_attitude(&att_sp, &att, &rates_sp, NULL); orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); -// printf("publish attitude\n"); } diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c index c25e96856a..2129915d12 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -312,10 +312,6 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s actuators->control[3] = motor_thrust; } -// if(motor_skip_counter%20 == 0) -// printf("MAC: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",actuators->control[0], actuators->control[1], actuators->control[2], actuators->control[3]); - - // XXX change yaw rate to yaw pos controller if (rates_sp) { rates_sp->roll = roll_control; @@ -324,8 +320,5 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s rates_sp->thrust = motor_thrust; } -// if(motor_skip_counter%20 == 0) -// printf("Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",rates_sp->roll, rates_sp->pitch, rates_sp->yaw, rates_sp->thrust); - motor_skip_counter++; } diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index ceb8c4b104..f81dfa9b8f 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -1030,7 +1030,6 @@ Sensors::ppm_poll() orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc); orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control); -// printf("SENSORS: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",manual_control.roll, manual_control.pitch, manual_control.yaw, manual_control.throttle); } #endif