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

Rover: queue MISSION_ITEM_REACHED 

clean up unreachable code
This commit is contained in:
Tom Pittenger 2015-07-24 00:31:30 -07:00 committed by Randy Mackay
parent 945639ac0f
commit e7bd6a46ca
3 changed files with 26 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
APMrover2/GCS_Mavlink.cpp
View File

@ -5,10 +5,6 @@
// default sensors are present and healthy: gyro, accelerometer, 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_ANGULAR_RATE_CONTROL | MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION | MAV_SYS_STATUS_SENSOR_YAW_POSITION | 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 Rover::send_heartbeat(mavlink_channel_t chan)
{
uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
@ -416,8 +412,6 @@ bool Rover::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 (rover.telemetry_delayed(chan)) {
return false;
}
@ -590,6 +584,11 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
rover.send_pid_tuning(chan);
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:
case MSG_TERRAIN:
case MSG_OPTICAL_FLOW:
@ -1321,6 +1320,19 @@ void Rover::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 Rover::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
APMrover2/Rover.h
View File

@ -396,6 +396,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);

8
APMrover2/commands_logic.cpp
View File

@ -124,10 +124,16 @@ void Rover::exit_mission()
// verify_command_callback - callback function called from ap-mission at 10hz or higher when a command is being run
// we double check that the flight mode is AUTO to avoid the possibility of ap-mission triggering actions while we're not in AUTO mode
bool Plane::verify_command_callback(const AP_Mission::Mission_Command& cmd)
bool Rover::verify_command_callback(const AP_Mission::Mission_Command& cmd)
{
if (control_mode == AUTO) {
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;