From 2e0b55d887c82aeeb473c73fd87aa49de333b289 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sun, 27 Nov 2011 19:11:44 -0800 Subject: [PATCH] Cosmetic cleanup, commented functions for compiler warnings --- ArduCopter/GCS_Mavlink.pde | 34 +++++++++++++++++----------------- ArduCopter/motors.pde | 2 +- ArduCopter/motors_quad.pde | 2 +- ArduCopter/navigation.pde | 13 +++++++------ 4 files changed, 26 insertions(+), 25 deletions(-) diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index ef42c8e131..14e8cf1ce3 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -687,7 +687,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) switch (msg->msgid) { - case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: + case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: //66 { // decode mavlink_request_data_stream_t packet; @@ -762,7 +762,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } - case MAVLINK_MSG_ID_ACTION: + case MAVLINK_MSG_ID_ACTION: //10 { // decode mavlink_action_t packet; @@ -895,7 +895,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } - case MAVLINK_MSG_ID_SET_MODE: + case MAVLINK_MSG_ID_SET_MODE: //11 { // decode mavlink_set_mode_t packet; @@ -934,7 +934,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } */ - case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST: + case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST: //43 { //send_text_P(SEVERITY_LOW,PSTR("waypoint request list")); @@ -959,7 +959,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) } // XXX read a WP from EEPROM and send it to the GCS - case MAVLINK_MSG_ID_WAYPOINT_REQUEST: + case MAVLINK_MSG_ID_WAYPOINT_REQUEST: // 40 { //send_text_P(SEVERITY_LOW,PSTR("waypoint request")); @@ -1081,7 +1081,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } - case MAVLINK_MSG_ID_WAYPOINT_ACK: + case MAVLINK_MSG_ID_WAYPOINT_ACK: //47 { //send_text_P(SEVERITY_LOW,PSTR("waypoint ack")); @@ -1095,7 +1095,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } - case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: + case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: // 21 { //send_text_P(SEVERITY_LOW,PSTR("param request list")); @@ -1112,7 +1112,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } - case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL: + case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL: // 45 { //send_text_P(SEVERITY_LOW,PSTR("waypoint clear all")); @@ -1132,7 +1132,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } - case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: + case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: // 41 { //send_text_P(SEVERITY_LOW,PSTR("waypoint set current")); @@ -1148,7 +1148,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } - case MAVLINK_MSG_ID_WAYPOINT_COUNT: + case MAVLINK_MSG_ID_WAYPOINT_COUNT: // 44 { //send_text_P(SEVERITY_LOW,PSTR("waypoint count")); @@ -1183,7 +1183,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) #endif // XXX receive a WP from GCS and store in EEPROM - case MAVLINK_MSG_ID_WAYPOINT: + case MAVLINK_MSG_ID_WAYPOINT: //39 { // decode mavlink_waypoint_t packet; @@ -1347,7 +1347,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } - case MAVLINK_MSG_ID_PARAM_SET: + case MAVLINK_MSG_ID_PARAM_SET: // 23 { AP_Var *vp; AP_Meta_class::Type_id var_type; @@ -1430,7 +1430,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } // end case - case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: + case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: //70 { // allow override of RC channel values for HIL // or for complete GCS control of switch position @@ -1458,7 +1458,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) #if HIL_MODE != HIL_MODE_DISABLED // This is used both as a sensor and to pass the location // in HIL_ATTITUDE mode. - case MAVLINK_MSG_ID_GPS_RAW: + case MAVLINK_MSG_ID_GPS_RAW: //32 { // decode mavlink_gps_raw_t packet; @@ -1479,7 +1479,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } #if HIL_MODE == HIL_MODE_ATTITUDE - case MAVLINK_MSG_ID_ATTITUDE: + case MAVLINK_MSG_ID_ATTITUDE: //30 { // decode mavlink_attitude_t packet; @@ -1546,7 +1546,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) */ #if HIL_MODE == HIL_MODE_SENSORS - case MAVLINK_MSG_ID_RAW_IMU: + case MAVLINK_MSG_ID_RAW_IMU: // 28 { // decode mavlink_raw_imu_t packet; @@ -1578,7 +1578,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } - case MAVLINK_MSG_ID_RAW_PRESSURE: + case MAVLINK_MSG_ID_RAW_PRESSURE: //29 { // decode mavlink_raw_pressure_t packet; diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 01f3a263a6..add12f49b6 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -82,7 +82,7 @@ static void init_arm_motors() init_simple_bearing(); // Reset home position - // ---------------------- + // ------------------- if(home_is_set) init_home(); diff --git a/ArduCopter/motors_quad.pde b/ArduCopter/motors_quad.pde index 887b10675d..94afb640f9 100644 --- a/ArduCopter/motors_quad.pde +++ b/ArduCopter/motors_quad.pde @@ -126,7 +126,7 @@ static void output_motors_disarmed() } /* -static void debug_motors() +//static void debug_motors() { Serial.printf("1:%d\t2:%d\t3:%d\t4:%d\n", motor_out[CH_1], diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 7b41ed13e3..88b94813b1 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -67,7 +67,6 @@ static void calc_loiter(int x_error, int y_error) int y_target_speed = g.pi_loiter_lat.get_pi(y_error, dTnav); // find the rates: - //float temp = radians((float)g_gps->ground_course/100.0); float temp = g_gps->ground_course * RADX100; #ifdef OPTFLOW_ENABLED @@ -255,7 +254,8 @@ static int32_t get_altitude_error() return next_WP.alt - current_loc.alt; } -/*static int get_loiter_angle() +/* +//static int get_loiter_angle() { float power; int angle; @@ -288,7 +288,7 @@ static int32_t wrap_180(int32_t error) } /* -static int32_t get_crosstrack_correction(void) +//static int32_t get_crosstrack_correction(void) { // Crosstrack Error // ---------------- @@ -307,19 +307,20 @@ static int32_t get_crosstrack_correction(void) } */ /* -static int32_t cross_track_test() +//static int32_t cross_track_test() { int32_t temp = wrap_180(target_bearing - crosstrack_bearing); return abs(temp); } */ /* -static void reset_crosstrack() +//static void reset_crosstrack() { crosstrack_bearing = get_bearing(¤t_loc, &next_WP); // Used for track following } */ -/*static int32_t get_altitude_above_home(void) +/* +//static int32_t get_altitude_above_home(void) { // This is the altitude above the home location // The GPS gives us altitude at Sea Level