/* * send notification that a mission command has been completed */ void GCS_MAVLINK::send_mission_item_reached(uint16_t seq) const { mavlink_msg_mission_item_reached_send(chan, seq); }
// try to send a message, return false if it won't fit in the serial tx buffer bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id) { if (telemetry_delayed(chan)) { return false; } // if we don't have at least 1ms remaining before the main loop // wants to fire then don't send a mavlink message. We want to // prioritise the main flight control loop over communications if (!rover.in_mavlink_delay && rover.scheduler.time_available_usec() < 1200) { rover.gcs_out_of_time = true; return false; } switch (id) { case MSG_HEARTBEAT: CHECK_PAYLOAD_SIZE(HEARTBEAT); last_heartbeat_time = AP_HAL::millis(); rover.send_heartbeat(chan); return true; case MSG_EXTENDED_STATUS1: CHECK_PAYLOAD_SIZE(SYS_STATUS); rover.send_extended_status1(chan); CHECK_PAYLOAD_SIZE(POWER_STATUS); send_power_status(); break; case MSG_EXTENDED_STATUS2: CHECK_PAYLOAD_SIZE(MEMINFO); send_meminfo(); break; case MSG_ATTITUDE: CHECK_PAYLOAD_SIZE(ATTITUDE); rover.send_attitude(chan); break; case MSG_LOCATION: CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT); rover.send_location(chan); break; case MSG_LOCAL_POSITION: CHECK_PAYLOAD_SIZE(LOCAL_POSITION_NED); send_local_position(rover.ahrs); break; case MSG_NAV_CONTROLLER_OUTPUT: if (rover.control_mode != MANUAL) { CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT); rover.send_nav_controller_output(chan); } break; case MSG_GPS_RAW: CHECK_PAYLOAD_SIZE(GPS_RAW_INT); send_gps_raw(rover.gps); break; case MSG_SYSTEM_TIME: CHECK_PAYLOAD_SIZE(SYSTEM_TIME); send_system_time(rover.gps); break; case MSG_SERVO_OUT: CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED); rover.send_servo_out(chan); break; case MSG_RADIO_IN: CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW); send_radio_in(rover.receiver_rssi); break; case MSG_RADIO_OUT: CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW); send_servo_output_raw(false); break; case MSG_VFR_HUD: CHECK_PAYLOAD_SIZE(VFR_HUD); rover.send_vfr_hud(chan); break; case MSG_RAW_IMU1: CHECK_PAYLOAD_SIZE(RAW_IMU); send_raw_imu(rover.ins, rover.compass); break; case MSG_RAW_IMU3: CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS); send_sensor_offsets(rover.ins, rover.compass, rover.barometer); break; case MSG_CURRENT_WAYPOINT: CHECK_PAYLOAD_SIZE(MISSION_CURRENT); rover.send_current_waypoint(chan); break; case MSG_NEXT_PARAM: CHECK_PAYLOAD_SIZE(PARAM_VALUE); queued_param_send(); break; case MSG_NEXT_WAYPOINT: CHECK_PAYLOAD_SIZE(MISSION_REQUEST); queued_waypoint_send(); break; case MSG_STATUSTEXT: // depreciated, use GCS_MAVLINK::send_statustext* return false; case MSG_AHRS: CHECK_PAYLOAD_SIZE(AHRS); send_ahrs(rover.ahrs); break; case MSG_SIMSTATE: CHECK_PAYLOAD_SIZE(SIMSTATE); rover.send_simstate(chan); break; case MSG_HWSTATUS: CHECK_PAYLOAD_SIZE(HWSTATUS); rover.send_hwstatus(chan); break; case MSG_RANGEFINDER: CHECK_PAYLOAD_SIZE(RANGEFINDER); rover.send_rangefinder(chan); break; case MSG_MOUNT_STATUS: #if MOUNT == ENABLED CHECK_PAYLOAD_SIZE(MOUNT_STATUS); rover.camera_mount.status_msg(chan); #endif // MOUNT == ENABLED break; case MSG_RAW_IMU2: case MSG_LIMITS_STATUS: case MSG_FENCE_STATUS: case MSG_WIND: // unused break; case MSG_VIBRATION: CHECK_PAYLOAD_SIZE(VIBRATION); send_vibration(rover.ins); break; case MSG_BATTERY2: CHECK_PAYLOAD_SIZE(BATTERY2); send_battery2(rover.battery); break; case MSG_CAMERA_FEEDBACK: #if CAMERA == ENABLED CHECK_PAYLOAD_SIZE(CAMERA_FEEDBACK); rover.camera.send_feedback(chan, rover.gps, rover.ahrs, rover.current_loc); #endif break; case MSG_EKF_STATUS_REPORT: #if AP_AHRS_NAVEKF_AVAILABLE CHECK_PAYLOAD_SIZE(EKF_STATUS_REPORT); rover.ahrs.send_ekf_status_report(chan); #endif break; case MSG_PID_TUNING: CHECK_PAYLOAD_SIZE(PID_TUNING); 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_MAG_CAL_PROGRESS: CHECK_PAYLOAD_SIZE(MAG_CAL_PROGRESS); rover.compass.send_mag_cal_progress(chan); break; case MSG_MAG_CAL_REPORT: CHECK_PAYLOAD_SIZE(MAG_CAL_REPORT); rover.compass.send_mag_cal_report(chan); break; case MSG_RETRY_DEFERRED: case MSG_ADSB_VEHICLE: case MSG_TERRAIN: case MSG_OPTICAL_FLOW: case MSG_GIMBAL_REPORT: case MSG_RPM: case MSG_POSITION_TARGET_GLOBAL_INT: break; // just here to prevent a warning } return true; }
void MAVMissionOutput_40hz(void) { #if (FLIGHT_PLAN_TYPE == FP_WAYPOINTS) // LOGO_WAYPOINTS cannot be uploaded / downloaded vect3_32t wp; if (mavlink_flags.mavlink_send_waypoint_reached == 1) { mavlink_flags.mavlink_send_waypoint_reached = 0; mavlink_msg_mission_item_reached_send(MAVLINK_COMM_0, mav_waypoint_reached); } if (mavlink_flags.mavlink_send_waypoint_changed == 1) { mavlink_flags.mavlink_send_waypoint_changed = 0; mavlink_msg_mission_current_send(MAVLINK_COMM_0, mav_waypoint_changed); } //static inline void mavlink_msg_mission_item_reached_send(mavlink_channel_t chan, uint16_t seq) //static inline void mavlink_msg_mission_current_send(mavlink_channel_t chan, uint16_t seq) // CHECK WHETHER WAYPOINT PROTOCOL HAS TIMED OUT WAITING ON A RESPONSE if (mavlink_waypoint_timeout <= 0) { if (mavlink_flags.mavlink_sending_waypoints || mavlink_flags.mavlink_receiving_waypoints) { //send_text((uint8_t *)"Timeout on waypoint protocol.\r\n"); DPRINT("Timeout on waypoint protocol.\r\n"); } mavlink_flags.mavlink_sending_waypoints = false; mavlink_flags.mavlink_receiving_waypoints = false; } // if (mavlink_flags.mavlink_receiving_waypoints == 1) if (mavlink_flags.mavlink_request_specific_waypoint == 1) { DPRINT("requesting waypoint: %u\r\n", waypoint_request_i); //mavlink_flags.waypoint_request_i = 0; //static inline void mavlink_msg_mission_request_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq) mavlink_msg_mission_request_send(MAVLINK_COMM_0, mavlink_waypoint_dest_sysid, mavlink_waypoint_dest_compid, waypoint_request_i); mavlink_flags.mavlink_request_specific_waypoint = 0; } // SEND NUMBER OF WAYPOINTS IN WAYPOINTS LIST if (mavlink_flags.mavlink_send_waypoint_count == 1) { int16_t number_of_waypoints = waypoint_count(); //send_text((uint8_t *)"Sending waypoint count\r\n"); DPRINT("Sending waypoint count: %u\r\n", number_of_waypoints); mavlink_msg_mission_count_send(MAVLINK_COMM_0, mavlink_waypoint_dest_sysid, mavlink_waypoint_dest_compid, number_of_waypoints); mavlink_flags.mavlink_send_waypoint_count = 0; } // SEND DETAILS OF A SPECIFIC WAYPOINT if (mavlink_flags.mavlink_send_specific_waypoint == 1) { //send_text((uint8_t *)"Time to send a specific waypoint\r\n"); DPRINT("Time to send a specific waypoint: %u\r\n", mavlink_waypoint_requested_sequence_number); // mavlink_msg_mission_item_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, // uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, // float param1, float param2, float param3, float param4, // float x, float y, float z) //BUILDING //struct waypoint3D { int32_t x; int32_t y; int16_t z; }; // struct waypoint3D getWaypoint3D(uint16_t wp); // struct waypoint3D wp; // wp = getWaypoint3D(mavlink_waypoint_requested_sequence_number); wp = getWaypoint3D(mavlink_waypoint_requested_sequence_number); //float lat_float, lon_float, alt_float = 0.0; //uint32_t accum_long = IMUlocationy._.W1 + (lat_origin.WW / 90); // meters North from Equator //lat_float = (float)((accum_long * 90) / 10000000.0); // degrees North from Equator //lon_float = (float)((float) lon_origin.WW + ((float)(IMUlocationx._.W1) * 90.0) / (float)(cos_lat / 16384.0)) / 10000000.0; //extern struct relWaypointDef wp_to_relative(struct waypointDef wp); //struct relWaypointDef current_waypoint = wp_to_relative(waypoints[waypointIndex]); //alt_float = ((float)(IMUlocationz._.W1)) + (float)(alt_origin.WW / 100.0); mavlink_msg_mission_item_send(MAVLINK_COMM_0, mavlink_waypoint_dest_sysid, mavlink_waypoint_dest_compid, \ mavlink_waypoint_requested_sequence_number, mavlink_waypoint_frame, MAV_CMD_NAV_WAYPOINT, mavlink_waypoint_current, true, \ 0.0, 0.0, 0.0, 0.0, \ (float)wp.y / 10000000.0, (float)wp.x / 10000000.0, wp.z); DPRINT("waypoint %f %f %f\r\n", (double)wp.y / 10000000.0, (double)wp.x / 10000000.0, (double)wp.z); mavlink_flags.mavlink_send_specific_waypoint = 0; } if (mavlink_waypoint_timeout > 0) mavlink_waypoint_timeout--; #endif // (FLIGHT_PLAN_TYPE == FP_WAYPOINTS) /* // Acknowledge a command if flaged to do so. if (mavlink_send_command_ack == true) { mavlink_msg_command_ack_send(MAVLINK_COMM_0, mavlink_command_ack_command, mavlink_command_ack_result); mavlink_send_command_ack = false; } */ }