void Create_packets(ExtU_quadrotor_FCS_0_T *data,int sock) { int bytes_sent; unsigned int temp = 0; int i; uint8_t buf[BUFFER_LENGTH]; mavlink_message_t msg; uint16_t len; mavlink_msg_heartbeat_pack(3, 200, &msg, 2, 12, 65, 0, 3); len = mavlink_msg_to_send_buffer(buf, &msg); int len1 = strlen(buf); bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); mavlink_msg_attitude_pack(3, 200, &msg, microsSinceEpoch(),data->Internalstates[3], data->Internalstates[4], data->Internalstates[5], data->Internalstates[6], data->Internalstates[7], data->Internalstates[8]); len = mavlink_msg_to_send_buffer(buf, &msg); len1 = strlen(buf); bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); mavlink_msg_gps_raw_int_pack(3, 200, &msg, microsSinceEpoch(),2,data->lla[0]*10000000,data->lla[1]*10000000,data->lla[2],UINT16_MAX,UINT16_MAX,UINT16_MAX,UINT16_MAX,255); len = mavlink_msg_to_send_buffer(buf, &msg); bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); mavlink_msg_rc_channels_pack(3, 200, &msg, microsSinceEpoch(),4,data->RC[1]*100,data->RC[2]*100,data->RC[3]*100,data->RC[4]*100,data->ORC[0],data->ORC[1],data->ORC[2],data->ORC[3],data->rpm[0],data->rpm[1],data->rpm[2],data->rpm[3],UINT16_MAX,UINT16_MAX,UINT16_MAX,UINT16_MAX,UINT16_MAX,UINT16_MAX,255); len = mavlink_msg_to_send_buffer(buf, &msg); bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); memset(buf, 0, BUFFER_LENGTH); }
/** * Pull the raw GPS sensor data from the gpsDataStore struct within the GPS module and * transmit it via MAVLink over UART1. This function should only be called when the GPS * data has been updated. * TODO: Convert this message to a GLOBAL_POSITION_INT */ void MavLinkSendRawGps(void) { mavlink_message_t msg; mavlink_msg_gps_raw_int_pack(mavlink_system.sysid, mavlink_system.compid, &msg, ((uint64_t)systemStatus.time)*10000, sensorAvailability.gps.active?3:0, (int32_t)(gpsDataStore.lat.flData*180/M_PI*1e7), (int32_t)(gpsDataStore.lon.flData*180/M_PI*1e7), (int32_t)(gpsDataStore.alt.flData*1e7), 0xFFFF, 0xFFFF, (uint16_t)gpsDataStore.sog.flData*100, (uint16_t)gpsDataStore.cog.flData * 100, 0xFF); len = mavlink_msg_to_send_buffer(buf, &msg); uart1EnqueueData(buf, (uint8_t)len); }
uint16_t PackGpsRawInt(uint8_t system_id, uint8_t component_id, mavlink_gps_raw_int_t mlRawGpsDataInt ,uint32_t time_usec){ mavlink_system_t mavlink_system; //if (!(mlPending.wpProtState == WP_PROT_IDLE)) // return 0; mavlink_system.sysid = system_id; ///< ID 20 for this airplane mavlink_system.compid = component_id;//MAV_COMP_ID_IMU; ///< The component sending the message is the IMU, it could be also a Linux process ////////////////////////////////////////////////////////////////////////// mavlink_message_t msg; memset(&msg, 0, sizeof (mavlink_message_t)); mavlink_msg_gps_raw_int_pack(mavlink_system.sysid, mavlink_system.compid, &msg , time_usec ,mlRawGpsDataInt.fix_type, mlRawGpsDataInt.lat, mlRawGpsDataInt.lon, mlRawGpsDataInt.alt, mlRawGpsDataInt.eph, mlRawGpsDataInt.epv, mlRawGpsDataInt.vel, mlRawGpsDataInt.cog, mlRawGpsDataInt.satellites_visible); return( mavlink_msg_to_send_buffer(UartOutBuff, &msg)); }
void cDataLink::SendGpsMsg(unsigned short fix_type, long lat, long lon, long alt, long vdop, long hdop, long vel, long course, unsigned short sats) { int bytes_sent; uint16_t len; /* Send GPS raw int */ mavlink_msg_gps_raw_int_pack(1, MAV_COMP_ID_GPS, &m_msg, microsSinceEpoch(), fix_type, //fix_type lat, lon, alt, //lat, lon, alt vdop, hdop, // hdop, vdop vel, //vel*100 course, //course deg*100 sats); //visible satellites len = mavlink_msg_to_send_buffer(m_buf, &m_msg); bytes_sent = sendto(sock, m_buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); (void)bytes_sent; //avoid compiler warning }
void Create_packets(ExtU_ANN_EKF_NMPC_2_T *data,int sock) { int i=0; int bytes_sent; uint8_t buf[BUFFER_LENGTH]; uint8_t buf1[BUFFER_LENGTH]; uint8_t buf2[BUFFER_LENGTH]; mavlink_message_t msg,msg1,msg2; uint16_t len; mavlink_msg_heartbeat_pack(2, 200, &msg, 2, 12, 65, 0, 3); len = mavlink_msg_to_send_buffer(buf, &msg); int len1 = strlen(buf); bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); mavlink_msg_attitude_pack(2, 200, &msg, microsSinceEpoch(),data->EulerAnglesmeas.phi, data->EulerAnglesmeas.theta, data->EulerAnglesmeas.psi, data->BodyRatesmeas.P,data->BodyRatesmeas.Q, data->BodyRatesmeas.R); len = mavlink_msg_to_send_buffer(buf, &msg); len1 = strlen(buf); bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); // mavlink_msg_hil_state_pack(1, 200, &msg, microsSinceEpoch(),data->EulerAngles.phi, data->EulerAngles.theta, data->EulerAngles.psi, data->BodyRatesmeas.P, data->BodyRatesmeas.Q, data->BodyRatesmeas.R,data->GPSPosition.Latitude,data->GPSPosition.Longitude,data->GPSPosition.Altitude,0,0,0,data->Accelerometermeas.Axb,data->Accelerometermeas.Ayb,data->Accelerometermeas.Azb); // mavlink_msg_hil_state_pack(2, 200, &msg, microsSinceEpoch(),data->EulerAngles.phi, data->EulerAngles.theta, data->EulerAngles.psi, data->BodyRatesmeas.P, data->BodyRatesmeas.Q, data->BodyRatesmeas.R,39,-95,data->GPSPosition.Altitude,0,0,0,data->Accelerometermeas.Axb,data->Accelerometermeas.Ayb,data->Accelerometermeas.Azb); //len = mavlink_msg_to_send_buffer(buf, &msg); // bytes_sent = write(tty_fd1,buf,len); mavlink_msg_gps_raw_int_pack(2, 200, &msg, microsSinceEpoch(),2,data->GPSPositionmeas.Latitude,data->GPSPositionmeas.Longitude,data->GPSPositionmeas.Altitude,UINT16_MAX,UINT16_MAX,UINT16_MAX,UINT16_MAX,255); len = mavlink_msg_to_send_buffer(buf, &msg); bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); memset(buf, 0, BUFFER_LENGTH); //mavlink_message_t msg; mavlink_status_t status,status1; mavlink_mission_count_t mission_count; int cn; recsize = recvfrom(sock, (void *)buf, BUFFER_LENGTH, 0, (struct sockaddr *)&gcAddr, &fromlen); if (recsize > 0) { // Something received - print out all bytes and parse packet printf("Bytes Received: %d\nDatagram: ", (int)recsize); for (i = 0; i < recsize; ++i) { //temp = buf[i]; // printf("%02x ", (unsigned char)temp); if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status)) { // Packet received printf("\nReceived packet: SYS: %d, COMP: %d, LEN: %d, MSG ID: %d\n", msg.sysid, msg.compid, msg.len, msg.msgid); printf("\n\n Incomming packet\n\n"); if(msg.msgid == 44) { mavlink_msg_mission_count_decode( &msg, &mission_count); printf("\n\n the # of wypts received is %d########\n\n",mission_count.count); data->wcn=mission_count.count; for(cn=1;cn<=data->wcn;cn++) { //sendrequest(sock,cn); memset(buf1, 0, BUFFER_LENGTH); mavlink_msg_mission_request_pack(2, 200, &msg1,255,0,(cn-1)); len = mavlink_msg_to_send_buffer(buf1, &msg1); bytes_sent = sendto(sock, buf1, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); //printf("\n\n @@@@@ sent successfull\n\n"); memset(buf2, 0, BUFFER_LENGTH); //receive_waypoints(sock,cn); while( (recsize1 = recvfrom(sock, (void *)buf2, BUFFER_LENGTH, 0, (struct sockaddr *)&gcAddr, &fromlen)) <= 0); if (recsize1 > 0) { int ii; mavlink_mission_item_t mission_item; for (ii = 0; ii < recsize1; ++ii) { if (mavlink_parse_char(MAVLINK_COMM_0, buf2[ii], &msg2, &status1)) { printf("\n\n reading waypoint # %d",(cn-1)); } }//if (msg2.msgid==0) //goto loop; if(msg2.msgid == 39) { mavlink_msg_mission_item_decode(&msg2, &mission_item); printf("\n\nthe waypoint # %d is lat:%f lon:%f alt:%f",cn,mission_item.x,mission_item.y,mission_item.z); data->lat[cn]=mission_item.x; data->alt[cn] =mission_item.z; data->lon[cn] = mission_item.y; data->WaypointsIN.v[cn] = mission_item.param1; latlon(&ANN_EKF_NMPC_2_U,cn); } }else printf("!!!!!!!!\n\n receiving failed \n\n "); } if((cn-1) == mission_count.count) { //sendack(sock,cn); mavlink_msg_mission_ack_pack(2, 200, &msg2,255,0,0); memset(buf2, 0, BUFFER_LENGTH); len = mavlink_msg_to_send_buffer(buf2, &msg2); bytes_sent = sendto(sock, buf2, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); printf("\n\n ***** sent acknowledgement *****\n\n"); } } // for msg id 44 if(msg.msgid == 43) { int j; //sendcn(sock,cn); mavlink_msg_mission_count_pack(2, 200, &msg2,255,0,data->wcn); memset(buf2, 0, BUFFER_LENGTH); len = mavlink_msg_to_send_buffer(buf2, &msg2); bytes_sent = sendto(sock, buf2, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); memset(buf2, 0, BUFFER_LENGTH); for(j=1; j<=data->wcn;j++) { //receive_request(sock); //sendwyp(sock,-95,95); mavlink_msg_mission_item_pack(2, 200, &msg2,255,0,(j-1),0,16,0,1,data->WaypointsIN.v[j],0,0,0,data->lat[j],data->lon[j],data->alt[j]); len = mavlink_msg_to_send_buffer(buf2, &msg2); bytes_sent = sendto(sock, buf2, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); printf("\n\n @@@@@ sent count successfull\n\n"); } //rec_ack(sock); }// for msg id 43 } //parse - if }//for }//else printf("\n\n 12121212not good");//recsize - if // printf("\n"); //memset(buf, 0, BUFFER_LENGTH); //printf("\n\n !!!@@##$$ end of a loop\n\n"); }//for Createpacket
static void uavoMavlinkBridgeTask(void *parameters) { uint32_t lastSysTime; // Main task loop lastSysTime = PIOS_Thread_Systime(); FlightBatterySettingsData batSettings = {}; if (FlightBatterySettingsHandle() != NULL ) FlightBatterySettingsGet(&batSettings); SystemStatsData systemStats; while (1) { PIOS_Thread_Sleep_Until(&lastSysTime, 1000 / TASK_RATE_HZ); if (stream_trigger(MAV_DATA_STREAM_EXTENDED_STATUS)) { FlightBatteryStateData batState = {}; if (FlightBatteryStateHandle() != NULL ) FlightBatteryStateGet(&batState); SystemStatsGet(&systemStats); int8_t battery_remaining = 0; if (batSettings.Capacity != 0) { if (batState.ConsumedEnergy < batSettings.Capacity) { battery_remaining = 100 - lroundf(batState.ConsumedEnergy / batSettings.Capacity * 100); } } uint16_t voltage = 0; if (batSettings.VoltagePin != FLIGHTBATTERYSETTINGS_VOLTAGEPIN_NONE) voltage = lroundf(batState.Voltage * 1000); uint16_t current = 0; if (batSettings.CurrentPin != FLIGHTBATTERYSETTINGS_CURRENTPIN_NONE) current = lroundf(batState.Current * 100); mavlink_msg_sys_status_pack(0, 200, mav_msg, // onboard_control_sensors_present Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control 0, // onboard_control_sensors_enabled Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control 0, // onboard_control_sensors_health Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control 0, // load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 (uint16_t)systemStats.CPULoad * 10, // voltage_battery Battery voltage, in millivolts (1 = 1 millivolt) voltage, // current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current current, // battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery battery_remaining, // drop_rate_comm Communication drops in percent, (0%: 0, 100%: 10'000), (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) 0, // errors_comm Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) 0, // errors_count1 Autopilot-specific errors 0, // errors_count2 Autopilot-specific errors 0, // errors_count3 Autopilot-specific errors 0, // errors_count4 Autopilot-specific errors 0); send_message(); } if (stream_trigger(MAV_DATA_STREAM_RC_CHANNELS)) { ManualControlCommandData manualState; FlightStatusData flightStatus; ManualControlCommandGet(&manualState); FlightStatusGet(&flightStatus); SystemStatsGet(&systemStats); //TODO connect with RSSI object and pass in last argument mavlink_msg_rc_channels_raw_pack(0, 200, mav_msg, // time_boot_ms Timestamp (milliseconds since system boot) systemStats.FlightTime, // port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. 0, // chan1_raw RC channel 1 value, in microseconds manualState.Channel[0], // chan2_raw RC channel 2 value, in microseconds manualState.Channel[1], // chan3_raw RC channel 3 value, in microseconds manualState.Channel[2], // chan4_raw RC channel 4 value, in microseconds manualState.Channel[3], // chan5_raw RC channel 5 value, in microseconds manualState.Channel[4], // chan6_raw RC channel 6 value, in microseconds manualState.Channel[5], // chan7_raw RC channel 7 value, in microseconds manualState.Channel[6], // chan8_raw RC channel 8 value, in microseconds manualState.Channel[7], // rssi Receive signal strength indicator, 0: 0%, 255: 100% manualState.Rssi); send_message(); } if (stream_trigger(MAV_DATA_STREAM_POSITION)) { GPSPositionData gpsPosData = {}; HomeLocationData homeLocation = {}; SystemStatsGet(&systemStats); if (GPSPositionHandle() != NULL ) GPSPositionGet(&gpsPosData); if (HomeLocationHandle() != NULL ) HomeLocationGet(&homeLocation); SystemStatsGet(&systemStats); uint8_t gps_fix_type; switch (gpsPosData.Status) { case GPSPOSITION_STATUS_NOGPS: gps_fix_type = 0; break; case GPSPOSITION_STATUS_NOFIX: gps_fix_type = 1; break; case GPSPOSITION_STATUS_FIX2D: gps_fix_type = 2; break; case GPSPOSITION_STATUS_FIX3D: case GPSPOSITION_STATUS_DIFF3D: gps_fix_type = 3; break; default: gps_fix_type = 0; break; } mavlink_msg_gps_raw_int_pack(0, 200, mav_msg, // time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t)systemStats.FlightTime * 1000, // fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. gps_fix_type, // lat Latitude in 1E7 degrees gpsPosData.Latitude, // lon Longitude in 1E7 degrees gpsPosData.Longitude, // alt Altitude in 1E3 meters (millimeters) above MSL gpsPosData.Altitude * 1000, // eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 gpsPosData.HDOP * 100, // epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 gpsPosData.VDOP * 100, // vel GPS ground speed (m/s * 100). If unknown, set to: 65535 gpsPosData.Groundspeed * 100, // cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 gpsPosData.Heading * 100, // satellites_visible Number of satellites visible. If unknown, set to 255 gpsPosData.Satellites); send_message(); mavlink_msg_gps_global_origin_pack(0, 200, mav_msg, // latitude Latitude (WGS84), expressed as * 1E7 homeLocation.Latitude, // longitude Longitude (WGS84), expressed as * 1E7 homeLocation.Longitude, // altitude Altitude(WGS84), expressed as * 1000 homeLocation.Altitude * 1000); send_message(); //TODO add waypoint nav stuff //wp_target_bearing //wp_dist = mavlink_msg_nav_controller_output_get_wp_dist(&msg); //alt_error = mavlink_msg_nav_controller_output_get_alt_error(&msg); //aspd_error = mavlink_msg_nav_controller_output_get_aspd_error(&msg); //xtrack_error = mavlink_msg_nav_controller_output_get_xtrack_error(&msg); //mavlink_msg_nav_controller_output_pack //wp_number //mavlink_msg_mission_current_pack } if (stream_trigger(MAV_DATA_STREAM_EXTRA1)) { AttitudeActualData attActual; SystemStatsData systemStats; AttitudeActualGet(&attActual); SystemStatsGet(&systemStats); mavlink_msg_attitude_pack(0, 200, mav_msg, // time_boot_ms Timestamp (milliseconds since system boot) systemStats.FlightTime, // roll Roll angle (rad) attActual.Roll * DEG2RAD, // pitch Pitch angle (rad) attActual.Pitch * DEG2RAD, // yaw Yaw angle (rad) attActual.Yaw * DEG2RAD, // rollspeed Roll angular speed (rad/s) 0, // pitchspeed Pitch angular speed (rad/s) 0, // yawspeed Yaw angular speed (rad/s) 0); send_message(); } if (stream_trigger(MAV_DATA_STREAM_EXTRA2)) { ActuatorDesiredData actDesired; AttitudeActualData attActual; AirspeedActualData airspeedActual = {}; GPSPositionData gpsPosData = {}; BaroAltitudeData baroAltitude = {}; FlightStatusData flightStatus; if (AirspeedActualHandle() != NULL ) AirspeedActualGet(&airspeedActual); if (GPSPositionHandle() != NULL ) GPSPositionGet(&gpsPosData); if (BaroAltitudeHandle() != NULL ) BaroAltitudeGet(&baroAltitude); ActuatorDesiredGet(&actDesired); AttitudeActualGet(&attActual); FlightStatusGet(&flightStatus); float altitude = 0; if (BaroAltitudeHandle() != NULL) altitude = baroAltitude.Altitude; else if (GPSPositionHandle() != NULL) altitude = gpsPosData.Altitude; // round attActual.Yaw to nearest int and transfer from (-180 ... 180) to (0 ... 360) int16_t heading = lroundf(attActual.Yaw); if (heading < 0) heading += 360; mavlink_msg_vfr_hud_pack(0, 200, mav_msg, // airspeed Current airspeed in m/s airspeedActual.TrueAirspeed, // groundspeed Current ground speed in m/s gpsPosData.Groundspeed, // heading Current heading in degrees, in compass units (0..360, 0=north) heading, // throttle Current throttle setting in integer percent, 0 to 100 actDesired.Throttle * 100, // alt Current altitude (MSL), in meters altitude, // climb Current climb rate in meters/second 0); send_message(); uint8_t armed_mode = 0; if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) armed_mode |= MAV_MODE_FLAG_SAFETY_ARMED; uint8_t custom_mode = CUSTOM_MODE_STAB; switch (flightStatus.FlightMode) { case FLIGHTSTATUS_FLIGHTMODE_MANUAL: case FLIGHTSTATUS_FLIGHTMODE_MWRATE: case FLIGHTSTATUS_FLIGHTMODE_VIRTUALBAR: case FLIGHTSTATUS_FLIGHTMODE_HORIZON: /* Kinda a catch all */ custom_mode = CUSTOM_MODE_SPORT; break; case FLIGHTSTATUS_FLIGHTMODE_ACRO: case FLIGHTSTATUS_FLIGHTMODE_AXISLOCK: custom_mode = CUSTOM_MODE_ACRO; break; case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: /* May want these three to try and * infer based on roll axis */ case FLIGHTSTATUS_FLIGHTMODE_LEVELING: custom_mode = CUSTOM_MODE_STAB; break; case FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE: custom_mode = CUSTOM_MODE_DRIFT; break; case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD: custom_mode = CUSTOM_MODE_ALTH; break; case FLIGHTSTATUS_FLIGHTMODE_RETURNTOHOME: custom_mode = CUSTOM_MODE_RTL; break; case FLIGHTSTATUS_FLIGHTMODE_TABLETCONTROL: case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: custom_mode = CUSTOM_MODE_POSH; break; case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: custom_mode = CUSTOM_MODE_AUTO; break; } mavlink_msg_heartbeat_pack(0, 200, mav_msg, // type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) MAV_TYPE_GENERIC, // autopilot Autopilot type / class. defined in MAV_AUTOPILOT ENUM MAV_AUTOPILOT_GENERIC, // base_mode System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h armed_mode, // custom_mode A bitfield for use for autopilot-specific flags. custom_mode, // system_status System status flag, see MAV_STATE ENUM 0); send_message(); } } }
void mavlinkSendPosition(void) { uint16_t msgLength; uint8_t gpsFixType = 0; if (!sensors(SENSOR_GPS)) return; if (!STATE(GPS_FIX)) { gpsFixType = 1; } else { if (GPS_numSat < 5) { gpsFixType = 2; } else { gpsFixType = 3; } } mavlink_msg_gps_raw_int_pack(0, 200, &mavMsg, // time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) micros(), // fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. gpsFixType, // lat Latitude in 1E7 degrees GPS_coord[LAT], // lon Longitude in 1E7 degrees GPS_coord[LON], // alt Altitude in 1E3 meters (millimeters) above MSL GPS_altitude * 1000, // eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 65535, // epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 65535, // vel GPS ground speed (m/s * 100). If unknown, set to: 65535 GPS_speed, // cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 GPS_ground_course * 10, // satellites_visible Number of satellites visible. If unknown, set to 255 GPS_numSat); msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg); mavlinkSerialWrite(mavBuffer, msgLength); // Global position mavlink_msg_global_position_int_pack(0, 200, &mavMsg, // time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) micros(), // lat Latitude in 1E7 degrees GPS_coord[LAT], // lon Longitude in 1E7 degrees GPS_coord[LON], // alt Altitude in 1E3 meters (millimeters) above MSL GPS_altitude * 1000, // relative_alt Altitude above ground in meters, expressed as * 1000 (millimeters) #if defined(BARO) || defined(SONAR) (sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) ? altitudeHoldGetEstimatedAltitude() * 10 : GPS_altitude * 1000, #else GPS_altitude * 1000, #endif // Ground X Speed (Latitude), expressed as m/s * 100 0, // Ground Y Speed (Longitude), expressed as m/s * 100 0, // Ground Z Speed (Altitude), expressed as m/s * 100 0, // heading Current heading in degrees, in compass units (0..360, 0=north) DECIDEGREES_TO_DEGREES(attitude.values.yaw) ); msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg); mavlinkSerialWrite(mavBuffer, msgLength); mavlink_msg_gps_global_origin_pack(0, 200, &mavMsg, // latitude Latitude (WGS84), expressed as * 1E7 GPS_home[LAT], // longitude Longitude (WGS84), expressed as * 1E7 GPS_home[LON], // altitude Altitude(WGS84), expressed as * 1000 0); msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg); mavlinkSerialWrite(mavBuffer, msgLength); }
void mavlinkSendPosition(timeUs_t currentTimeUs) { uint16_t msgLength; uint8_t gpsFixType = 0; if (!sensors(SENSOR_GPS)) return; if (gpsSol.fixType == GPS_NO_FIX) gpsFixType = 1; else if (gpsSol.fixType == GPS_FIX_2D) gpsFixType = 2; else if (gpsSol.fixType == GPS_FIX_3D) gpsFixType = 3; mavlink_msg_gps_raw_int_pack(0, 200, &mavMsg, // time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) currentTimeUs, // fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. gpsFixType, // lat Latitude in 1E7 degrees gpsSol.llh.lat, // lon Longitude in 1E7 degrees gpsSol.llh.lon, // alt Altitude in 1E3 meters (millimeters) above MSL gpsSol.llh.alt * 10, // eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 65535, // epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 65535, // vel GPS ground speed (m/s * 100). If unknown, set to: 65535 gpsSol.groundSpeed, // cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 gpsSol.groundCourse * 10, // satellites_visible Number of satellites visible. If unknown, set to 255 gpsSol.numSat); msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg); mavlinkSerialWrite(mavBuffer, msgLength); // Global position mavlink_msg_global_position_int_pack(0, 200, &mavMsg, // time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) currentTimeUs, // lat Latitude in 1E7 degrees gpsSol.llh.lat, // lon Longitude in 1E7 degrees gpsSol.llh.lon, // alt Altitude in 1E3 meters (millimeters) above MSL gpsSol.llh.alt * 10, // relative_alt Altitude above ground in meters, expressed as * 1000 (millimeters) #if defined(NAV) getEstimatedActualPosition(Z) * 10, #else gpsSol.llh.alt * 10, #endif // Ground X Speed (Latitude), expressed as m/s * 100 0, // Ground Y Speed (Longitude), expressed as m/s * 100 0, // Ground Z Speed (Altitude), expressed as m/s * 100 0, // heading Current heading in degrees, in compass units (0..360, 0=north) DECIDEGREES_TO_DEGREES(attitude.values.yaw) ); msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg); mavlinkSerialWrite(mavBuffer, msgLength); mavlink_msg_gps_global_origin_pack(0, 200, &mavMsg, // latitude Latitude (WGS84), expressed as * 1E7 GPS_home.lat, // longitude Longitude (WGS84), expressed as * 1E7 GPS_home.lon, // altitude Altitude(WGS84), expressed as * 1000 GPS_home.alt * 10); // FIXME msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg); mavlinkSerialWrite(mavBuffer, msgLength); }