// ------------------------------------------------------------------------------ // Write Heartbeat message // ------------------------------------------------------------------------------ void Autopilot_Interface:: write_heartbeat() { // -------------------------------------------------------------------------- // PACK PAYLOAD // -------------------------------------------------------------------------- // Allocate Space mavlink_message_t msg; mavlink_heartbeat_t heartbeat; struct timespec t_now_rt,t_diff_rt; uint64_t timestamp = get_time_usec(); // Pack the message //mavlink_msg_heartbeat_pack(system_id, companion_id, &msg, mav_type, autopilot_id , base_mode, custom_mode, system_status) heartbeat.type = MAV_TYPE_GCS; heartbeat.autopilot = autopilot_id; heartbeat.base_mode = base_mode; heartbeat.system_status = system_status; heartbeat.custom_mode = 0; mavlink_msg_heartbeat_encode(system_id, companion_id, &msg, &heartbeat); //mavlink_msg_heartbeat_encode(2, 125, &msg, &heartbeat); // -------------------------------------------------------------------------- // WRITE // -------------------------------------------------------------------------- clock_gettime(CLOCK_REALTIME,&t_now_rt); int len = write_message(msg); timespec_sub(&t_diff_rt,&t_now_rt,&t_wrold_rt); int64_t diff_rt = t_diff_rt.tv_sec*1e6 + t_diff_rt.tv_nsec/1e3; //clock_gettime(CLOCK_REALTIME,&t_wrold_rt); // With this command I obtained a DT with the writing time in the computation t_wrold_rt.tv_nsec = t_now_rt.tv_nsec; t_wrold_rt.tv_sec = t_now_rt.tv_sec; //printf("hil_sensors_t DT : %lu us\r",diff_rt); // check the write if ( not len > 0 ) fprintf(stderr,"WARNING: could not send HIL_SENSORS \n"); // else // printf("%lu POSITION_TARGET = [ %f , %f , %f ] \n", write_count, position_target.x, position_target.y, position_target.z); return; }
void mavlink_connector::slow_send() { mavlink_gps_raw_int_t gps_raw_int_t; mavlink_battery_status_t battery_status_t; mavlink_rc_channels_t rc_channels_t; mavlink_message_t msg; gps_raw_int_t.lat = (int32_t)(dji_variable::global_position_degree.lati * 1e7) ; gps_raw_int_t.lon = (int32_t)(dji_variable::global_position_degree.longti * 1e7); gps_raw_int_t.alt = (int32_t)(dji_variable::global_position_degree.alti * 1000); mavlink_msg_gps_raw_int_encode(0,200,&msg,&gps_raw_int_t); send_msg(&msg); mavlink_msg_heartbeat_encode(0,200,&msg,make_heartbeat()); send_msg(&msg); // printf("sended heartbeating......\n"); battery_status_t.battery_remaining = dji_variable::battery; for (int i = 0 ;i< 6;i++) battery_status_t.voltages[i] = (uint16_t)((float)dji_variable::battery / 100.0f * 22.2 * 1000); mavlink_msg_battery_status_encode(0,200,&msg,&battery_status_t); send_msg(&msg); mavlink_sys_status_t sys_status_t; memset(&sys_status_t,0, sizeof(sys_status_t)); sys_status_t.battery_remaining = dji_variable::battery; sys_status_t.voltage_battery = 222 * dji_variable::battery ; mavlink_msg_sys_status_encode(0,200,&msg,&sys_status_t); send_msg(&msg); mavlink_rc_channels_scaled_t rc_channels_scaled_t; rc_channels_scaled_t.chan1_scaled = dji_variable::rc_channels.roll; rc_channels_scaled_t.chan2_scaled = dji_variable::rc_channels.pitch; rc_channels_scaled_t.chan3_scaled = dji_variable::rc_channels.throttle; rc_channels_scaled_t.chan4_scaled = dji_variable::rc_channels.yaw; rc_channels_scaled_t.chan5_scaled = dji_variable::rc_channels.mode; rc_channels_scaled_t.chan6_scaled = dji_variable::rc_channels.gear; mavlink_msg_rc_channels_scaled_encode(0,200,&msg,&rc_channels_scaled_t); send_msg(&msg); }
//called periodically Void cron_quick_clock(UArg arg){ // flash led GPIO_toggle(Board_LED_GREEN); // use red led for user inputs /* MAVLINK HEARTBEAT */ // Initialize the message buffer static COMM_FRAME frame; mavlink_heartbeat_t hb=comm_get_mavlink_heartbeat(); // Pack the message mavlink_msg_heartbeat_encode(mavlink_system.sysid, mavlink_system.compid, &(frame.mavlink_message), &hb); comm_mavlink_broadcast(&frame); //send heartbeat for all available channel slots }
void Test::sendHearbeat(bool armed, MAV_STATE state) { mavlink_heartbeat_t hearbeat; hearbeat.autopilot = MAV_AUTOPILOT_ARDUPILOTMEGA; hearbeat.base_mode = 0; if (armed) { hearbeat.base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } hearbeat.custom_mode = Aircraft::CustomMode_PositionHold; hearbeat.mavlink_version = 1; hearbeat.system_status = state; hearbeat.type = MAV_TYPE_QUADROTOR; // hearbeat.type = MAV_TYPE_GCS; mavlink_message_t buf; mavlink_msg_heartbeat_encode(SYSTEM_ID, COMPONENT_ID, &buf, &hearbeat); testMessage(buf); }
/* send a report to the vehicle control code over MAVLink */ void Gimbal::send_report(void) { if (!mavlink.connected && mav_socket.connect(target_address, target_port)) { ::printf("Gimbal connected to %s:%u\n", target_address, (unsigned)target_port); mavlink.connected = true; } if (!mavlink.connected) { return; } // check for incoming MAVLink messages uint8_t buf[100]; ssize_t ret; while ((ret=mav_socket.recv(buf, sizeof(buf), 0)) > 0) { for (uint8_t i=0; i<ret; i++) { mavlink_message_t msg; mavlink_status_t status; if (mavlink_frame_char_buffer(&mavlink.rxmsg, &mavlink.status, buf[i], &msg, &status) == MAVLINK_FRAMING_OK) { switch (msg.msgid) { case MAVLINK_MSG_ID_HEARTBEAT: { if (!seen_heartbeat) { seen_heartbeat = true; vehicle_component_id = msg.compid; vehicle_system_id = msg.sysid; ::printf("Gimbal using srcSystem %u\n", (unsigned)vehicle_system_id); } break; } case MAVLINK_MSG_ID_GIMBAL_CONTROL: { mavlink_gimbal_control_t pkt; mavlink_msg_gimbal_control_decode(&msg, &pkt); demanded_angular_rate = Vector3f(pkt.demanded_rate_x, pkt.demanded_rate_y, pkt.demanded_rate_z); // no longer supply a bias supplied_gyro_bias.zero(); seen_gimbal_control = true; break; } } } } } if (!seen_heartbeat) { return; } uint32_t now = hal.scheduler->millis(); mavlink_message_t msg; uint16_t len; if (now - last_heartbeat_ms >= 1000) { mavlink_heartbeat_t heartbeat; heartbeat.type = MAV_TYPE_GIMBAL; heartbeat.autopilot = MAV_AUTOPILOT_ARDUPILOTMEGA; heartbeat.base_mode = 0; heartbeat.system_status = 0; heartbeat.mavlink_version = 0; heartbeat.custom_mode = 0; /* save and restore sequence number for chan0, as it is used by generated encode functions */ mavlink_status_t *chan0_status = mavlink_get_channel_status(MAVLINK_COMM_0); uint8_t saved_seq = chan0_status->current_tx_seq; chan0_status->current_tx_seq = mavlink.seq; len = mavlink_msg_heartbeat_encode(vehicle_system_id, vehicle_component_id, &msg, &heartbeat); chan0_status->current_tx_seq = saved_seq; mav_socket.send(&msg.magic, len); last_heartbeat_ms = now; } /* send a GIMBAL_REPORT message */ uint32_t now_us = hal.scheduler->micros(); if (now_us - last_report_us > reporting_period_ms*1000UL) { mavlink_gimbal_report_t gimbal_report; float delta_time = (now_us - last_report_us) * 1.0e-6f; last_report_us = now_us; gimbal_report.target_system = vehicle_system_id; gimbal_report.target_component = vehicle_component_id; gimbal_report.delta_time = delta_time; gimbal_report.delta_angle_x = delta_angle.x; gimbal_report.delta_angle_y = delta_angle.y; gimbal_report.delta_angle_z = delta_angle.z; gimbal_report.delta_velocity_x = delta_velocity.x; gimbal_report.delta_velocity_y = delta_velocity.y; gimbal_report.delta_velocity_z = delta_velocity.z; gimbal_report.joint_roll = joint_angles.x; gimbal_report.joint_el = joint_angles.y; gimbal_report.joint_az = joint_angles.z; mavlink_status_t *chan0_status = mavlink_get_channel_status(MAVLINK_COMM_0); uint8_t saved_seq = chan0_status->current_tx_seq; chan0_status->current_tx_seq = mavlink.seq; len = mavlink_msg_gimbal_report_encode(vehicle_system_id, vehicle_component_id, &msg, &gimbal_report); chan0_status->current_tx_seq = saved_seq; mav_socket.send(&msg.magic, len); delta_velocity.zero(); delta_angle.zero(); } }
void loop(void) { uint16_t err_count = 0; // incoming heartbeat mavlink_message_t msg; mavlink_heartbeat_t heartbeat = {0}; mavlink_msg_heartbeat_encode(3, 1, &msg, &heartbeat); if (!routing.check_and_forward(MAVLINK_COMM_0, &msg)) { hal.console->printf("heartbeat should be processed locally\n"); err_count++; } // incoming non-targetted message mavlink_attitude_t attitude = {0}; mavlink_msg_attitude_encode(3, 1, &msg, &attitude); if (!routing.check_and_forward(MAVLINK_COMM_0, &msg)) { hal.console->printf("attitude should be processed locally\n"); err_count++; } // incoming targeted message for someone else mavlink_param_set_t param_set = {0}; param_set.target_system = mavlink_system.sysid+1; param_set.target_component = mavlink_system.compid; mavlink_msg_param_set_encode(3, 1, &msg, ¶m_set); if (routing.check_and_forward(MAVLINK_COMM_0, &msg)) { hal.console->printf("param set 1 should not be processed locally\n"); err_count++; } // incoming targeted message for us param_set.target_system = mavlink_system.sysid; param_set.target_component = mavlink_system.compid; mavlink_msg_param_set_encode(3, 1, &msg, ¶m_set); if (!routing.check_and_forward(MAVLINK_COMM_0, &msg)) { hal.console->printf("param set 2 should be processed locally\n"); err_count++; } // incoming targeted message for our system, but other compid // should be processed locally param_set.target_system = mavlink_system.sysid; param_set.target_component = mavlink_system.compid+1; mavlink_msg_param_set_encode(3, 1, &msg, ¶m_set); if (!routing.check_and_forward(MAVLINK_COMM_0, &msg)) { hal.console->printf("param set 3 should be processed locally\n"); err_count++; } // incoming broadcast message should be processed locally param_set.target_system = 0; param_set.target_component = mavlink_system.compid+1; mavlink_msg_param_set_encode(3, 1, &msg, ¶m_set); if (!routing.check_and_forward(MAVLINK_COMM_0, &msg)) { hal.console->printf("param set 4 should be processed locally\n"); err_count++; } if (err_count == 0) { hal.console->printf("All OK\n"); } hal.scheduler->delay(1000); }
/* send a report to the vehicle control code over MAVLink */ void ADSB::send_report(void) { if (AP_HAL::millis() < 10000) { // simulated aircraft don't appear until 10s after startup. This avoids a windows // threading issue with non-blocking sockets and the initial wait on uartA return; } if (!mavlink.connected && mav_socket.connect(target_address, target_port)) { ::printf("ADSB connected to %s:%u\n", target_address, (unsigned)target_port); mavlink.connected = true; } if (!mavlink.connected) { return; } // check for incoming MAVLink messages uint8_t buf[100]; ssize_t ret; while ((ret=mav_socket.recv(buf, sizeof(buf), 0)) > 0) { for (uint8_t i=0; i<ret; i++) { mavlink_message_t msg; mavlink_status_t status; if (mavlink_frame_char_buffer(&mavlink.rxmsg, &mavlink.status, buf[i], &msg, &status) == MAVLINK_FRAMING_OK) { switch (msg.msgid) { case MAVLINK_MSG_ID_HEARTBEAT: { if (!seen_heartbeat) { seen_heartbeat = true; vehicle_component_id = msg.compid; vehicle_system_id = msg.sysid; ::printf("ADSB using srcSystem %u\n", (unsigned)vehicle_system_id); } break; } } } } } if (!seen_heartbeat) { return; } uint32_t now = AP_HAL::millis(); mavlink_message_t msg; uint16_t len; if (now - last_heartbeat_ms >= 1000) { mavlink_heartbeat_t heartbeat; heartbeat.type = MAV_TYPE_ADSB; heartbeat.autopilot = MAV_AUTOPILOT_ARDUPILOTMEGA; heartbeat.base_mode = 0; heartbeat.system_status = 0; heartbeat.mavlink_version = 0; heartbeat.custom_mode = 0; /* save and restore sequence number for chan0, as it is used by generated encode functions */ mavlink_status_t *chan0_status = mavlink_get_channel_status(MAVLINK_COMM_0); uint8_t saved_seq = chan0_status->current_tx_seq; chan0_status->current_tx_seq = mavlink.seq; len = mavlink_msg_heartbeat_encode(vehicle_system_id, vehicle_component_id, &msg, &heartbeat); chan0_status->current_tx_seq = saved_seq; mav_socket.send(&msg.magic, len); last_heartbeat_ms = now; } /* send a ADSB_VEHICLE messages */ uint32_t now_us = AP_HAL::micros(); if (now_us - last_report_us >= reporting_period_ms*1000UL) { for (uint8_t i=0; i<num_vehicles; i++) { ADSB_Vehicle &vehicle = vehicles[i]; Location loc = home; location_offset(loc, vehicle.position.x, vehicle.position.y); // re-init when exceeding radius range if (get_distance(home, loc) > _sitl->adsb_radius_m) { vehicle.initialised = false; } mavlink_adsb_vehicle_t adsb_vehicle {}; last_report_us = now_us; adsb_vehicle.ICAO_address = vehicle.ICAO_address; adsb_vehicle.lat = loc.lat; adsb_vehicle.lon = loc.lng; adsb_vehicle.altitude_type = ADSB_ALTITUDE_TYPE_PRESSURE_QNH; adsb_vehicle.altitude = -vehicle.position.z * 1000; adsb_vehicle.heading = wrap_360_cd(100*degrees(atan2f(vehicle.velocity_ef.y, vehicle.velocity_ef.x))); adsb_vehicle.hor_velocity = norm(vehicle.velocity_ef.x, vehicle.velocity_ef.y) * 100; adsb_vehicle.ver_velocity = -vehicle.velocity_ef.z * 100; memcpy(adsb_vehicle.callsign, vehicle.callsign, sizeof(adsb_vehicle.callsign)); adsb_vehicle.emitter_type = ADSB_EMITTER_TYPE_LARGE; adsb_vehicle.tslc = 1; adsb_vehicle.flags = ADSB_FLAGS_VALID_COORDS | ADSB_FLAGS_VALID_ALTITUDE | ADSB_FLAGS_VALID_HEADING | ADSB_FLAGS_VALID_VELOCITY | ADSB_FLAGS_VALID_CALLSIGN | ADSB_FLAGS_SIMULATED; adsb_vehicle.squawk = 0; // NOTE: ADSB_FLAGS_VALID_SQUAWK bit is not set mavlink_status_t *chan0_status = mavlink_get_channel_status(MAVLINK_COMM_0); uint8_t saved_seq = chan0_status->current_tx_seq; chan0_status->current_tx_seq = mavlink.seq; len = mavlink_msg_adsb_vehicle_encode(vehicle_system_id, MAV_COMP_ID_ADSB, &msg, &adsb_vehicle); chan0_status->current_tx_seq = saved_seq; uint8_t msgbuf[len]; len = mavlink_msg_to_send_buffer(msgbuf, &msg); if (len > 0) { mav_socket.send(msgbuf, len); } } } // ADSB_transceiever is enabled, send the status report. if (_sitl->adsb_tx && now - last_tx_report_ms > 1000) { last_tx_report_ms = now; mavlink_status_t *chan0_status = mavlink_get_channel_status(MAVLINK_COMM_0); uint8_t saved_seq = chan0_status->current_tx_seq; uint8_t saved_flags = chan0_status->flags; chan0_status->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1; chan0_status->current_tx_seq = mavlink.seq; const mavlink_uavionix_adsb_transceiver_health_report_t health_report = {UAVIONIX_ADSB_RF_HEALTH_OK}; len = mavlink_msg_uavionix_adsb_transceiver_health_report_encode(vehicle_system_id, MAV_COMP_ID_ADSB, &msg, &health_report); chan0_status->current_tx_seq = saved_seq; chan0_status->flags = saved_flags; uint8_t msgbuf[len]; len = mavlink_msg_to_send_buffer(msgbuf, &msg); if (len > 0) { mav_socket.send(msgbuf, len); ::printf("ADSBsim send tx health packet\n"); } } }
// ------------------------------------------------------------------------------ // Top // ------------------------------------------------------------------------------ int top (int argc, char **arv) { // -------------------------------------------------------------------------- // SETUP!! // -------------------------------------------------------------------------- // sudo apt-get install socat (only once) // sudo chown <user> /dev (only run once) // socat -d -d pty,raw,echo=0,link=/dev/ttyVA00 pty,raw,echo=0,link=/dev/ttyUSB0 // or!! // ./start_pseudo_mavlink.sh starts everthing // -------------------------------------------------------------------------- // Start Port // -------------------------------------------------------------------------- char *uart_name = (char*)"/dev/ttyVA00"; // pseudo teletype int baudrate = 57600; Serial_Port serial_port(uart_name,baudrate); serial_port.start(); // OWNSHIP POSITION AND VELOCITY, LOCAL_NED mavlink_local_position_ned_t lp; lp.time_boot_ms = (uint32_t) (get_time_usec()/1000); lp.x = 0.1; lp.y = 0.2; lp.z = 0.3; lp.vx = 0.4; lp.vy = 0.5; lp.vz = 0.0; mavlink_attitude_t att; att.roll = 0; att.pitch = 0; att.yaw = 0; att.rollspeed = 0; att.pitchspeed = 0; att.yawspeed = 0; mavlink_heartbeat_t hb; hb.custom_mode = 0; hb.type = 0; hb.autopilot = 0; hb.base_mode = 0; hb.system_status = 0; hb.mavlink_version = 0; while (1) { mavlink_message_t lp_m, att_m, hb_m; mavlink_msg_local_position_ned_encode(system_id, component_id, &lp_m, &lp); mavlink_msg_attitude_encode(system_id, component_id, &att_m, &att); mavlink_msg_heartbeat_encode(system_id, component_id, &hb_m, &hb); serial_port.write_message(lp_m); serial_port.write_message(att_m); serial_port.write_message(hb_m); usleep(0.5e6); //2Hz } return 0; }