mirror of https://github.com/ArduPilot/ardupilot
Cosmetic cleanup, commented functions for compiler warnings
This commit is contained in:
parent
afee053bac
commit
2e0b55d887
|
@ -687,7 +687,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
||||||
|
|
||||||
switch (msg->msgid) {
|
switch (msg->msgid) {
|
||||||
|
|
||||||
case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
|
case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: //66
|
||||||
{
|
{
|
||||||
// decode
|
// decode
|
||||||
mavlink_request_data_stream_t packet;
|
mavlink_request_data_stream_t packet;
|
||||||
|
@ -762,7 +762,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
case MAVLINK_MSG_ID_ACTION:
|
case MAVLINK_MSG_ID_ACTION: //10
|
||||||
{
|
{
|
||||||
// decode
|
// decode
|
||||||
mavlink_action_t packet;
|
mavlink_action_t packet;
|
||||||
|
@ -895,7 +895,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
case MAVLINK_MSG_ID_SET_MODE:
|
case MAVLINK_MSG_ID_SET_MODE: //11
|
||||||
{
|
{
|
||||||
// decode
|
// decode
|
||||||
mavlink_set_mode_t packet;
|
mavlink_set_mode_t packet;
|
||||||
|
@ -934,7 +934,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
||||||
break;
|
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"));
|
//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
|
// 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"));
|
//send_text_P(SEVERITY_LOW,PSTR("waypoint request"));
|
||||||
|
|
||||||
|
@ -1081,7 +1081,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
case MAVLINK_MSG_ID_WAYPOINT_ACK:
|
case MAVLINK_MSG_ID_WAYPOINT_ACK: //47
|
||||||
{
|
{
|
||||||
//send_text_P(SEVERITY_LOW,PSTR("waypoint ack"));
|
//send_text_P(SEVERITY_LOW,PSTR("waypoint ack"));
|
||||||
|
|
||||||
|
@ -1095,7 +1095,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
||||||
break;
|
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"));
|
//send_text_P(SEVERITY_LOW,PSTR("param request list"));
|
||||||
|
|
||||||
|
@ -1112,7 +1112,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
||||||
break;
|
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"));
|
//send_text_P(SEVERITY_LOW,PSTR("waypoint clear all"));
|
||||||
|
|
||||||
|
@ -1132,7 +1132,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
||||||
break;
|
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"));
|
//send_text_P(SEVERITY_LOW,PSTR("waypoint set current"));
|
||||||
|
|
||||||
|
@ -1148,7 +1148,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
case MAVLINK_MSG_ID_WAYPOINT_COUNT:
|
case MAVLINK_MSG_ID_WAYPOINT_COUNT: // 44
|
||||||
{
|
{
|
||||||
//send_text_P(SEVERITY_LOW,PSTR("waypoint count"));
|
//send_text_P(SEVERITY_LOW,PSTR("waypoint count"));
|
||||||
|
|
||||||
|
@ -1183,7 +1183,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
// XXX receive a WP from GCS and store in EEPROM
|
// XXX receive a WP from GCS and store in EEPROM
|
||||||
case MAVLINK_MSG_ID_WAYPOINT:
|
case MAVLINK_MSG_ID_WAYPOINT: //39
|
||||||
{
|
{
|
||||||
// decode
|
// decode
|
||||||
mavlink_waypoint_t packet;
|
mavlink_waypoint_t packet;
|
||||||
|
@ -1347,7 +1347,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
case MAVLINK_MSG_ID_PARAM_SET:
|
case MAVLINK_MSG_ID_PARAM_SET: // 23
|
||||||
{
|
{
|
||||||
AP_Var *vp;
|
AP_Var *vp;
|
||||||
AP_Meta_class::Type_id var_type;
|
AP_Meta_class::Type_id var_type;
|
||||||
|
@ -1430,7 +1430,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
||||||
break;
|
break;
|
||||||
} // end case
|
} // 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
|
// allow override of RC channel values for HIL
|
||||||
// or for complete GCS control of switch position
|
// 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
|
#if HIL_MODE != HIL_MODE_DISABLED
|
||||||
// This is used both as a sensor and to pass the location
|
// This is used both as a sensor and to pass the location
|
||||||
// in HIL_ATTITUDE mode.
|
// in HIL_ATTITUDE mode.
|
||||||
case MAVLINK_MSG_ID_GPS_RAW:
|
case MAVLINK_MSG_ID_GPS_RAW: //32
|
||||||
{
|
{
|
||||||
// decode
|
// decode
|
||||||
mavlink_gps_raw_t packet;
|
mavlink_gps_raw_t packet;
|
||||||
|
@ -1479,7 +1479,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
#if HIL_MODE == HIL_MODE_ATTITUDE
|
#if HIL_MODE == HIL_MODE_ATTITUDE
|
||||||
case MAVLINK_MSG_ID_ATTITUDE:
|
case MAVLINK_MSG_ID_ATTITUDE: //30
|
||||||
{
|
{
|
||||||
// decode
|
// decode
|
||||||
mavlink_attitude_t packet;
|
mavlink_attitude_t packet;
|
||||||
|
@ -1546,7 +1546,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
||||||
*/
|
*/
|
||||||
#if HIL_MODE == HIL_MODE_SENSORS
|
#if HIL_MODE == HIL_MODE_SENSORS
|
||||||
|
|
||||||
case MAVLINK_MSG_ID_RAW_IMU:
|
case MAVLINK_MSG_ID_RAW_IMU: // 28
|
||||||
{
|
{
|
||||||
// decode
|
// decode
|
||||||
mavlink_raw_imu_t packet;
|
mavlink_raw_imu_t packet;
|
||||||
|
@ -1578,7 +1578,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
case MAVLINK_MSG_ID_RAW_PRESSURE:
|
case MAVLINK_MSG_ID_RAW_PRESSURE: //29
|
||||||
{
|
{
|
||||||
// decode
|
// decode
|
||||||
mavlink_raw_pressure_t packet;
|
mavlink_raw_pressure_t packet;
|
||||||
|
|
|
@ -82,7 +82,7 @@ static void init_arm_motors()
|
||||||
init_simple_bearing();
|
init_simple_bearing();
|
||||||
|
|
||||||
// Reset home position
|
// Reset home position
|
||||||
// ----------------------
|
// -------------------
|
||||||
if(home_is_set)
|
if(home_is_set)
|
||||||
init_home();
|
init_home();
|
||||||
|
|
||||||
|
|
|
@ -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",
|
Serial.printf("1:%d\t2:%d\t3:%d\t4:%d\n",
|
||||||
motor_out[CH_1],
|
motor_out[CH_1],
|
||||||
|
|
|
@ -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);
|
int y_target_speed = g.pi_loiter_lat.get_pi(y_error, dTnav);
|
||||||
|
|
||||||
// find the rates:
|
// find the rates:
|
||||||
//float temp = radians((float)g_gps->ground_course/100.0);
|
|
||||||
float temp = g_gps->ground_course * RADX100;
|
float temp = g_gps->ground_course * RADX100;
|
||||||
|
|
||||||
#ifdef OPTFLOW_ENABLED
|
#ifdef OPTFLOW_ENABLED
|
||||||
|
@ -255,7 +254,8 @@ static int32_t get_altitude_error()
|
||||||
return next_WP.alt - current_loc.alt;
|
return next_WP.alt - current_loc.alt;
|
||||||
}
|
}
|
||||||
|
|
||||||
/*static int get_loiter_angle()
|
/*
|
||||||
|
//static int get_loiter_angle()
|
||||||
{
|
{
|
||||||
float power;
|
float power;
|
||||||
int angle;
|
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
|
// 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);
|
int32_t temp = wrap_180(target_bearing - crosstrack_bearing);
|
||||||
return abs(temp);
|
return abs(temp);
|
||||||
}
|
}
|
||||||
*/
|
*/
|
||||||
/*
|
/*
|
||||||
static void reset_crosstrack()
|
//static void reset_crosstrack()
|
||||||
{
|
{
|
||||||
crosstrack_bearing = get_bearing(¤t_loc, &next_WP); // Used for track following
|
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
|
// This is the altitude above the home location
|
||||||
// The GPS gives us altitude at Sea Level
|
// The GPS gives us altitude at Sea Level
|
||||||
|
|
Loading…
Reference in New Issue