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

Plane: implement try send mission_item_reached 

clean up unreachable code
This commit is contained in:
Tom Pittenger 2015-07-19 23:11:34 -07:00 committed by Randy Mackay
parent 7b611f8e26
commit 42061ee290
3 changed files with 27 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
ArduPlane/GCS_Mavlink.cpp
View File

@ -5,8 +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)
#define HAVE_PAYLOAD_SPACE(chan, id) (comm_get_txspace(chan) >= MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_ ## id ## _LEN)
void Plane::send_heartbeat(mavlink_channel_t chan)
{
uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
@ -570,15 +568,10 @@ bool Plane::telemetry_delayed(mavlink_channel_t chan)
return true;
}
// check if a message will fit in the payload space available
#define CHECK_PAYLOAD_SIZE(id) if (txspace < MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_ ## id ## _LEN) return false
#define CHECK_PAYLOAD_SIZE2(id) if (!HAVE_PAYLOAD_SPACE(chan, id)) return false
// 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 (plane.telemetry_delayed(chan)) {
return false;
}
@ -802,6 +795,11 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
case MSG_RPM:
// unused
break;
case MSG_MISSION_ITEM_REACHED:
CHECK_PAYLOAD_SIZE(MISSION_ITEM_REACHED);
mavlink_msg_mission_item_reached_send(chan, mission_item_reached_index);
break;
}
return true;
}
@ -1797,6 +1795,19 @@ void Plane::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 Plane::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
*/

1
ArduPlane/Plane.h
View File

@ -670,6 +670,7 @@ 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_update(void);
void gcs_send_text_P(gcs_severity severity, const prog_char_t *str);

12
ArduPlane/commands_logic.cpp
View File

@ -248,15 +248,12 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd) // Ret
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;
// do commands (always return true)
case MAV_CMD_DO_CHANGE_SPEED:
@ -857,7 +854,14 @@ bool Plane::start_command_callback(const AP_Mission::Mission_Command &cmd)
bool Plane::verify_command_callback(const AP_Mission::Mission_Command& cmd)
{
if (control_mode == AUTO) {
return verify_command(cmd);
bool cmd_complete = verify_command(cmd);
// send message to GCS
if (cmd_complete) {
gcs_send_mission_item_reached_message(cmd.index);
}
return cmd_complete;
}
return false;
}