/*
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();
}
}
/*
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");
}
}
}
