/* save the signing timestamp periodically */ void GCS_MAVLINK::save_signing_timestamp(bool force_save_now) { uint32_t now = AP_HAL::millis(); // we save the timestamp every 30s, unless forced by a GPS update if (!force_save_now && now - last_signing_save_ms < 30*1000UL) { return; } last_signing_save_ms = now; struct SigningKey key; if (!signing_key_load(key)) { return; } bool need_save = false; for (uint8_t i=0; i<MAVLINK_COMM_NUM_BUFFERS; i++) { mavlink_channel_t chan = (mavlink_channel_t)(MAVLINK_COMM_0 + i); const mavlink_status_t *status = mavlink_get_channel_status(chan); if (status && status->signing && status->signing->timestamp > key.timestamp) { key.timestamp = status->signing->timestamp; need_save = true; } } if (need_save) { // save updated key signing_key_save(key); } }
//TODO: handle full txspace properly void DataFlash_MAVLink::send_log_block(uint32_t block_address) { mavlink_channel_t chan = mavlink_channel_t(_chan - MAVLINK_COMM_0); if (!_initialised || comm_get_txspace(chan) < 255){ return; } mavlink_message_t msg; mavlink_status_t *chan_status = mavlink_get_channel_status(chan); uint8_t saved_seq = chan_status->current_tx_seq; chan_status->current_tx_seq = mavlink.seq++; Debug("Data Sent!!\n"); uint16_t len = mavlink_msg_remote_log_data_block_pack(mavlink.system_id, mavlink.component_id, &msg, 255, //GCS SYS ID 0, _block_max_size, _block_num[block_address], _buf[block_address]); if(comm_get_txspace(chan) < len){ return; } chan_status->current_tx_seq = saved_seq; _mavlink_resend_uart(chan, &msg); }
/* return true if signing is enabled on this channel */ bool GCS_MAVLINK::signing_enabled(void) const { const mavlink_status_t *status = mavlink_get_channel_status(chan); if (status->signing && (status->signing->flags & MAVLINK_SIGNING_FLAG_SIGN_OUTGOING)) { return true; } return false; }
/* load signing key */ void GCS_MAVLINK::load_signing_key(void) { struct SigningKey key; if (!signing_key_load(key)) { hal.console->printf("Failed to load signing key"); return; } mavlink_status_t *status = mavlink_get_channel_status(chan); if (status == NULL) { hal.console->printf("Failed to load signing key - no status"); return; } memcpy(signing.secret_key, key.secret_key, 32); signing.link_id = (uint8_t)chan; // use a timestamp 1 minute past the last recorded // timestamp. Combined with saving the key once every 30s this // prevents a window for replay attacks signing.timestamp = key.timestamp + 60UL * 100UL * 1000UL; signing.flags = MAVLINK_SIGNING_FLAG_SIGN_OUTGOING; signing.accept_unsigned_callback = accept_unsigned_callback; // if timestamp and key are all zero then we disable signing bool all_zero = (key.timestamp == 0); for (uint8_t i=0; i<sizeof(key.secret_key); i++) { if (signing.secret_key[i] != 0) { all_zero = false; } } // enable signing on all channels for (uint8_t i=0; i<MAVLINK_COMM_NUM_BUFFERS; i++) { mavlink_status_t *cstatus = mavlink_get_channel_status((mavlink_channel_t)(MAVLINK_COMM_0 + i)); if (cstatus != NULL) { if (all_zero) { // disable signing cstatus->signing = NULL; cstatus->signing_streams = NULL; } else { cstatus->signing = &signing; cstatus->signing_streams = &signing_streams; } } } }
//TODO: handle full txspace properly bool DataFlash_MAVLink::send_log_block(struct dm_block &block) { mavlink_channel_t chan = mavlink_channel_t(_chan - MAVLINK_COMM_0); if (!_initialised) { return false; } if (!HAVE_PAYLOAD_SPACE(chan, REMOTE_LOG_DATA_BLOCK)) { return false; } if (comm_get_txspace(chan) < 500) { return false; } #if CONFIG_HAL_BOARD == HAL_BOARD_SITL if (rand() < 0.1) { return false; } #endif #if DF_MAVLINK_DISABLE_INTERRUPTS irqstate_t istate = irqsave(); #endif // DM_packing: 267039 events, 0 overruns, 8440834us elapsed, 31us avg, min 31us max 32us 0.488us rms hal.util->perf_begin(_perf_packing); mavlink_message_t msg; mavlink_status_t *chan_status = mavlink_get_channel_status(chan); uint8_t saved_seq = chan_status->current_tx_seq; chan_status->current_tx_seq = mavlink_seq++; // Debug("Sending block (%d)", block.seqno); mavlink_msg_remote_log_data_block_pack(mavlink_system.sysid, MAV_COMP_ID_LOG, &msg, _target_system_id, _target_component_id, block.seqno, block.buf); hal.util->perf_end(_perf_packing); #if DF_MAVLINK_DISABLE_INTERRUPTS irqrestore(istate); #endif block.last_sent = AP_HAL::millis(); chan_status->current_tx_seq = saved_seq; // _last_send_time is set even if we fail to send the packet; if // the txspace is repeatedly chockas we should not add to the // problem and stop attempting to log _last_send_time = AP_HAL::millis(); _mavlink_resend_uart(chan, &msg); return true; }
static bool accept_unsigned_callback(const mavlink_status_t *status, uint32_t msgId) { if (status == mavlink_get_channel_status(MAVLINK_COMM_0)) { // always accept channel 0, assumed to be secure channel. This // is USB on PX4 boards return true; } for (uint8_t i=0; i<ARRAY_SIZE(accept_list); i++) { if (accept_list[i] == msgId) { return true; } } return false; }
static void shell_cmd_stats(char *args, void *data) { mavlink_status_t *status; unsigned char i; for (i = 0; i < MAVLINK_COMM_NUM_BUFFERS; i++) { status = mavlink_get_channel_status(i); shell_printf("\nMavlink channel %d\n", i); shell_printf(" msg_received=%d\n", status->msg_received); shell_printf(" packet_rx_drop_count=%d\n", status->packet_rx_drop_count); shell_printf(" packet_rx_success_count=%d\n", status->packet_rx_success_count); } shell_printf("\nActive channel mask=%x\n", active_channel_mask); shell_printf("\nUAV last seen %lums ago\n", get_millis() - uav_last_seen); }
//TODO: handle full txspace properly bool DataFlash_MAVLink::send_log_block(struct dm_block &block) { mavlink_channel_t chan = mavlink_channel_t(_chan - MAVLINK_COMM_0); if (!_initialised) { return false; } if (comm_get_txspace(chan) < MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK_LEN) { return false; } if (comm_get_txspace(chan) < 500) { return false; } #if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL if (rand() < 0.1) { return false; } #endif mavlink_message_t msg; mavlink_status_t *chan_status = mavlink_get_channel_status(chan); uint8_t saved_seq = chan_status->current_tx_seq; chan_status->current_tx_seq = mavlink_seq++; // Debug("Sending block (%d)", block.seqno); mavlink_msg_remote_log_data_block_pack(mavlink_system.sysid, MAV_COMP_ID_LOG, &msg, _target_system_id, _target_component_id, block.seqno, block.buf); block.last_sent = hal.scheduler->millis(); chan_status->current_tx_seq = saved_seq; // _last_send_time is set even if we fail to send the packet; if // the txspace is repeatedly chockas we should not add to the // problem and stop attempting to log _last_send_time = hal.scheduler->millis(); _mavlink_resend_uart(chan, &msg); return true; }
/* return packet overhead in bytes for a channel */ uint8_t GCS_MAVLINK::packet_overhead_chan(mavlink_channel_t chan) { /* reserve 100 bytes for parameters when a GCS fails to fetch a parameter due to lack of buffer space. The reservation lasts 2 seconds */ uint8_t reserved_space = 0; if (reserve_param_space_start_ms != 0 && AP_HAL::millis() - reserve_param_space_start_ms < 2000) { reserved_space = 100; } else { reserve_param_space_start_ms = 0; } const mavlink_status_t *status = mavlink_get_channel_status(chan); if (status->signing && (status->signing->flags & MAVLINK_SIGNING_FLAG_SIGN_OUTGOING)) { return MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_SIGNATURE_BLOCK_LEN + reserved_space; } return MAVLINK_NUM_NON_PAYLOAD_BYTES + reserved_space; }
/* update signing timestamp. This is called when we get GPS lock timestamp_usec is since 1/1/1970 (the epoch) */ void GCS_MAVLINK::update_signing_timestamp(uint64_t timestamp_usec) { uint64_t signing_timestamp = (timestamp_usec / (1000*1000ULL)); // this is the offset from 1/1/1970 to 1/1/2015 const uint64_t epoch_offset = 1420070400; if (signing_timestamp > epoch_offset) { signing_timestamp -= epoch_offset; } // convert to 10usec units signing_timestamp *= 100 * 1000ULL; // increase signing timestamp on any links that have signing for (uint8_t i=0; i<MAVLINK_COMM_NUM_BUFFERS; i++) { mavlink_channel_t chan = (mavlink_channel_t)(MAVLINK_COMM_0 + i); mavlink_status_t *status = mavlink_get_channel_status(chan); if (status && status->signing && status->signing->timestamp < signing_timestamp) { status->signing->timestamp = signing_timestamp; } } // save to stable storage save_signing_timestamp(true); }
/* 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 Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID) { /* If the wait until transmit flag is on, only transmit after we've received messages. Otherwise, transmit all the time. */ if (!should_transmit()) { return; } pthread_mutex_lock(&_send_mutex); unsigned buf_free = get_free_tx_buf(); uint8_t payload_len = mavlink_message_lengths[msgid]; unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES; _last_write_try_time = hrt_absolute_time(); /* check if there is space in the buffer, let it overflow else */ if (buf_free < packet_len) { /* no enough space in buffer to send */ count_txerr(); count_txerrbytes(packet_len); pthread_mutex_unlock(&_send_mutex); return; } uint8_t buf[MAVLINK_MAX_PACKET_LEN]; /* header */ buf[0] = MAVLINK_STX; buf[1] = payload_len; /* use mavlink's internal counter for the TX seq */ buf[2] = mavlink_get_channel_status(_channel)->current_tx_seq++; buf[3] = mavlink_system.sysid; buf[4] = (component_ID == 0) ? mavlink_system.compid : component_ID; buf[5] = msgid; /* payload */ memcpy(&buf[MAVLINK_NUM_HEADER_BYTES], msg, payload_len); /* checksum */ uint16_t checksum; crc_init(&checksum); crc_accumulate_buffer(&checksum, (const char *) &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len); crc_accumulate(mavlink_message_crcs[msgid], &checksum); buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF); buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8); /* send message to UART */ ssize_t ret = write(_uart_fd, buf, packet_len); if (ret != (int) packet_len) { count_txerr(); count_txerrbytes(packet_len); } else { _last_write_success_time = _last_write_try_time; count_txbytes(packet_len); } pthread_mutex_unlock(&_send_mutex); }
/* 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"); } } }
/* return true if the MAVLink parser is idle, so there is no partly parsed MAVLink message being processed */ bool comm_is_idle(mavlink_channel_t chan) { mavlink_status_t *status = mavlink_get_channel_status(chan); return status == NULL || status->parse_state <= MAVLINK_PARSE_STATE_IDLE; }
int main(void) { mavlink_channel_t chan; mavlink_test_all(11, 10, &last_msg); for (chan=MAVLINK_COMM_0; chan<=MAVLINK_COMM_1; chan++) { printf("Received %u messages on channel %u OK\n", chan_counts[chan], (unsigned)chan); } if (error_count != 0) { printf("Error count %u\n", error_count); exit(1); } printf("No errors detected\n"); #ifdef MAVLINK_SIGNING_FLAG_SIGN_OUTGOING mavlink_status_t *status; printf("Testing signing\n"); mavlink_signing_t signing; mavlink_signing_streams_t signing_streams; memset(&signing, 0, sizeof(signing)); memset(&signing_streams, 0, sizeof(signing_streams)); signing.flags = MAVLINK_SIGNING_FLAG_SIGN_OUTGOING; signing.link_id = 0; signing.timestamp = 1; memset(signing.secret_key, 42, sizeof(signing.secret_key)); status = mavlink_get_channel_status(MAVLINK_COMM_1); status->signing = &signing; status->signing_streams = &signing_streams; mavlink_test_all(11, 10, &last_msg); for (chan=MAVLINK_COMM_0; chan<=MAVLINK_COMM_1; chan++) { printf("Received %u messages on channel %u OK\n", chan_counts[chan], (unsigned)chan); } if (error_count != 0) { printf("Error count %u\n", error_count); exit(1); } printf("No errors detected\n"); #endif #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 status = mavlink_get_channel_status(MAVLINK_COMM_0); status->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1; status->signing = NULL; status = mavlink_get_channel_status(MAVLINK_COMM_1); status->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1; status->signing = NULL; printf("Testing sending as MAVLink1\n"); mavlink_test_all(11, 10, &last_msg); for (chan=MAVLINK_COMM_0; chan<=MAVLINK_COMM_1; chan++) { printf("Received %u messages on channel %u OK\n", chan_counts[chan], (unsigned)chan); } if (error_count != 0) { printf("Error count %u\n", error_count); exit(1); } printf("No errors detected\n"); #endif return 0; }