void FMStateMachine::_send_guide() { hal.console->println_P(PSTR("FollowMe: Sending guide waypoint packet")); int32_t lat = _local_gps_lat + _offs_lat; int32_t lon = _local_gps_lon + _offs_lon; // int32_t alt = _local_gps_altitude + _offs_altitude; // TODO: This is useless. It does not track altitude. It is constant. int32_t alt = _offs_altitude; /* assume above ground. (ArduCopter bug.) */ float x = (float) lat / (float) 1e7; /* lat, lon in deg * 10,000,000 */ float y = (float) lon / (float) 1e7; float z = (float) alt / (float) 100; /* alt in cm */ hal.console->printf_P( PSTR("FollowMe: guide x: %f y: %f z: %f\r\n"), x, y, z); mavlink_msg_mission_item_send( upstream_channel, /* mavlink_channel_t chan*/ _target_system, /* uint8_t target_system */ _target_component, /* uint8_t target_component */ 0, /* uint16_t seq: always 0, unknown why. */ MAV_FRAME_GLOBAL, /* uint8_t frame: arducopter uninterpreted */ MAV_CMD_NAV_WAYPOINT, /* uint16_t command: arducopter specific */ 2, /* uint8_t current: 2 indicates guided mode waypoint */ 0, /* uint8_t autocontinue: always 0 */ 0, /* float param1 : hold time in seconds */ 5, /* float param2 : acceptance radius in meters */ 0, /* float param3 : pass through waypoint */ 0, /* float param4 : desired yaw angle at waypoint */ x, /* float x : lat degrees */ y, /* float y : lon degrees */ z /* float z : alt meters */ ); }
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; } */ }