Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: implement try send mission_item_reached 

clean up unreachable code
This commit is contained in:
Tom Pittenger 2015-07-24 01:10:04 -07:00 committed by Andrew Tridgell
parent 86e3116fb6
commit 87d0b12c7c
3 changed files with 21 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/Copter.h
View File

@ -588,10 +588,10 @@ private:
void send_statustext(mavlink_channel_t chan);
bool telemetry_delayed(mavlink_channel_t chan);
void gcs_send_message(enum ap_message id);
void gcs_send_mission_item_reached_message(uint16_t mission_index);
void gcs_data_stream_send(void);
void gcs_check_input(void);
void gcs_send_text_P(gcs_severity severity, const prog_char_t *str);
void gcs_send_mission_item_reached(uint16_t seq);
void do_erase_logs(void);
void Log_Write_AutoTune(uint8_t axis, uint8_t tune_step, float meas_target, float meas_min, float meas_max, float new_gain_rp, float new_gain_rd, float new_gain_sp, float new_ddt);
void Log_Write_AutoTuneDetails(float angle_cd, float rate_cds);

35
ArduCopter/GCS_Mavlink.cpp
View File

@ -5,10 +5,6 @@
// default sensors are present and healthy: gyro, accelerometer, barometer, rate_control, attitude_stabilization, yaw_position, altitude control, x/y position control, motor_control
#define MAVLINK_SENSOR_PRESENT_DEFAULT (MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL | MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE | MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL | MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION | MAV_SYS_STATUS_SENSOR_YAW_POSITION | MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL | MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL | MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS | MAV_SYS_STATUS_AHRS)
// check if a message will fit in the payload space available
#define HAVE_PAYLOAD_SPACE(chan, id) (comm_get_txspace(chan) >= MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_ ## id ## _LEN)
#define CHECK_PAYLOAD_SIZE(id) if (txspace < MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_ ## id ## _LEN) return false
void Copter::gcs_send_heartbeat(void)
{
gcs_send_message(MSG_HEARTBEAT);
@ -518,8 +514,6 @@ bool Copter::telemetry_delayed(mavlink_channel_t chan)
// try to send a message, return false if it won't fit in the serial tx buffer
bool GCS_MAVLINK::try_send_message(enum ap_message id)
{
uint16_t txspace = comm_get_txspace(chan);
if (copter.telemetry_delayed(chan)) {
return false;
}
@ -738,6 +732,11 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
send_vibration(copter.ins);
break;
case MSG_MISSION_ITEM_REACHED:
CHECK_PAYLOAD_SIZE(MISSION_ITEM_REACHED);
mavlink_msg_mission_item_reached_send(chan, mission_item_reached_index);
break;
case MSG_RETRY_DEFERRED:
break; // just here to prevent a warning
}
@ -1841,6 +1840,19 @@ void Copter::gcs_send_message(enum ap_message id)
}
}
/*
* send a mission item reached message and load the index before the send attempt in case it may get delayed
*/
void Copter::gcs_send_mission_item_reached_message(uint16_t mission_index)
{
for (uint8_t i=0; i<num_gcs; i++) {
if (gcs[i].initialised) {
gcs[i].mission_item_reached_index = mission_index;
gcs[i].send_message(MSG_MISSION_ITEM_REACHED);
}
}
}
/*
* send data streams in the given rate range on both links
*/
@ -1900,14 +1912,3 @@ void Copter::gcs_send_text_fmt(const prog_char_t *fmt, ...)
}
}
/*
* send mission_item_reached message to all GCSs
*/
void Copter::gcs_send_mission_item_reached(uint16_t seq)
{
for (uint8_t i=0; i<num_gcs; i++) {
if (gcs[i].initialised) {
gcs[i].send_mission_item_reached(seq);
}
}
}

19
ArduCopter/commands_logic.cpp
View File

@ -168,7 +168,7 @@ bool Copter::verify_command_callback(const AP_Mission::Mission_Command& cmd)
// send message to GCS
if (cmd_complete) {
gcs_send_mission_item_reached(cmd.index);
gcs_send_mission_item_reached_message(cmd.index);
}
return cmd_complete;
@ -189,40 +189,31 @@ bool Copter::verify_command(const AP_Mission::Mission_Command& cmd)
//
case MAV_CMD_NAV_TAKEOFF:
return verify_takeoff();
break;
case MAV_CMD_NAV_WAYPOINT:
return verify_nav_wp(cmd);
break;
case MAV_CMD_NAV_LAND:
return verify_land();
break;
case MAV_CMD_NAV_LOITER_UNLIM:
return verify_loiter_unlimited();
break;
case MAV_CMD_NAV_LOITER_TURNS:
return verify_circle(cmd);
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_SPLINE_WAYPOINT:
return verify_spline_wp(cmd);
break;
#if NAV_GUIDED == ENABLED
case MAV_CMD_NAV_GUIDED_ENABLE:
return verify_nav_guided_enable(cmd);
break;
#endif
///
@ -230,29 +221,23 @@ bool Copter::verify_command(const AP_Mission::Mission_Command& cmd)
///
case MAV_CMD_CONDITION_DELAY:
return verify_wait_delay();
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_YAW:
return verify_yaw();
break;
case MAV_CMD_DO_PARACHUTE:
// assume parachute was released successfully
return true;
break;
default:
// return true if we do not recognise the command so that we move on to the next command
// return true if we do not recognize the command so that we move on to the next command
return true;
break;
}
}