void Ut_CReporterCoreDir::cleanup()
{
QDir::setCurrent(QDir::homePath());
QString rm = QString("rm -r -f %1 %2").arg(testMountPoint1).arg(testMountPoint2);
UNUSED_RESULT(system(rm.toLatin1()));
if (dir) {
delete dir;
dir = 0;
}
}
void panic(const char *errormsg, ...)
{
va_list ap;
va_start(ap, errormsg);
vdprintf(1, errormsg, ap);
va_end(ap);
UNUSED_RESULT(write(1, "\n", 1));
hal.rcin->deinit();
hal.scheduler->delay_microseconds(10000);
exit(1);
}
bool AP_Arming::can_checks(bool report)
{
#if HAL_WITH_UAVCAN
if (check_enabled(ARMING_CHECK_SYSTEM)) {
const char *fail_msg = nullptr;
uint8_t num_drivers = AP::can().get_num_drivers();
for (uint8_t i = 0; i < num_drivers; i++) {
switch (AP::can().get_protocol_type(i)) {
case AP_BoardConfig_CAN::Protocol_Type_KDECAN: {
// To be replaced with macro saying if KDECAN library is included
#if APM_BUILD_TYPE(APM_BUILD_ArduCopter) || APM_BUILD_TYPE(APM_BUILD_ArduPlane) || APM_BUILD_TYPE(APM_BUILD_ArduSub)
AP_KDECAN *ap_kdecan = AP_KDECAN::get_kdecan(i);
if (ap_kdecan != nullptr && !ap_kdecan->pre_arm_check(fail_msg)) {
if (fail_msg == nullptr) {
check_failed(ARMING_CHECK_SYSTEM, report, "KDECAN failed");
} else {
check_failed(ARMING_CHECK_SYSTEM, report, "%s", fail_msg);
}
return false;
}
break;
#else
UNUSED_RESULT(fail_msg); // prevent unused variable error
#endif
}
case AP_BoardConfig_CAN::Protocol_Type_UAVCAN:
case AP_BoardConfig_CAN::Protocol_Type_None:
default:
break;
}
}
}
#endif
return true;
}
// handle mavlink DISTANCE_SENSOR messages
void AP_Follow::handle_msg(const mavlink_message_t &msg)
{
// exit immediately if not enabled
if (!_enabled) {
return;
}
// skip our own messages
if (msg.sysid == mavlink_system.sysid) {
return;
}
// skip message if not from our target
if (_sysid != 0 && msg.sysid != _sysid) {
if (_automatic_sysid) {
// maybe timeout who we were following...
if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_SYSID_TIMEOUT_MS)) {
_sysid.set(0);
}
}
return;
}
// decode global-position-int message
if (msg.msgid == MAVLINK_MSG_ID_GLOBAL_POSITION_INT) {
// get estimated location and velocity (for logging)
Location loc_estimate{};
Vector3f vel_estimate;
UNUSED_RESULT(get_target_location_and_velocity(loc_estimate, vel_estimate));
// decode message
mavlink_global_position_int_t packet;
mavlink_msg_global_position_int_decode(&msg, &packet);
// ignore message if lat and lon are (exactly) zero
if ((packet.lat == 0 && packet.lon == 0)) {
return;
}
_target_location.lat = packet.lat;
_target_location.lng = packet.lon;
// select altitude source based on FOLL_ALT_TYPE param
if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
// relative altitude
_target_location.alt = packet.relative_alt / 10; // convert millimeters to cm
_target_location.relative_alt = 1; // set relative_alt flag
} else {
// absolute altitude
_target_location.alt = packet.alt / 10; // convert millimeters to cm
_target_location.relative_alt = 0; // reset relative_alt flag
}
_target_velocity_ned.x = packet.vx * 0.01f; // velocity north
_target_velocity_ned.y = packet.vy * 0.01f; // velocity east
_target_velocity_ned.z = packet.vz * 0.01f; // velocity down
// get a local timestamp with correction for transport jitter
_last_location_update_ms = _jitter.correct_offboard_timestamp_msec(packet.time_boot_ms, AP_HAL::millis());
if (packet.hdg <= 36000) { // heading (UINT16_MAX if unknown)
_target_heading = packet.hdg * 0.01f; // convert centi-degrees to degrees
_last_heading_update_ms = _last_location_update_ms;
}
// initialise _sysid if zero to sender's id
if (_sysid == 0) {
_sysid.set(msg.sysid);
_automatic_sysid = true;
}
// log lead's estimated vs reported position
AP::logger().Write("FOLL",
"TimeUS,Lat,Lon,Alt,VelN,VelE,VelD,LatE,LonE,AltE", // labels
"sDUmnnnDUm", // units
"F--B000--B", // mults
"QLLifffLLi", // fmt
AP_HAL::micros64(),
_target_location.lat,
_target_location.lng,
_target_location.alt,
(double)_target_velocity_ned.x,
(double)_target_velocity_ned.y,
(double)_target_velocity_ned.z,
loc_estimate.lat,
loc_estimate.lng,
loc_estimate.alt
);
}
}
// handle mavlink DISTANCE_SENSOR messages
void AP_Follow::handle_msg(const mavlink_message_t &msg)
{
// exit immediately if not enabled
if (!_enabled) {
return;
}
// skip our own messages
if (msg.sysid == mavlink_system.sysid) {
return;
}
// skip message if not from our target
if ((_sysid_to_follow != 0) && (msg.sysid != _sysid_to_follow)) {
if (_sysid == 0) {
// maybe timeout who we were following...
if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_SYSID_TIMEOUT_MS)) {
_sysid_to_follow = 0;
}
}
return;
}
// decode global-position-int message
if (msg.msgid == MAVLINK_MSG_ID_GLOBAL_POSITION_INT) {
const uint32_t now = AP_HAL::millis();
// get estimated location and velocity (for logging)
Location loc_estimate{};
Vector3f vel_estimate;
UNUSED_RESULT(get_target_location_and_velocity(loc_estimate, vel_estimate));
// decode message
mavlink_global_position_int_t packet;
mavlink_msg_global_position_int_decode(&msg, &packet);
// ignore message if lat and lon are (exactly) zero
if ((packet.lat == 0 && packet.lon == 0)) {
return;
}
_target_location.lat = packet.lat;
_target_location.lng = packet.lon;
// select altitude source based on FOLL_ALT_TYPE param
if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
// relative altitude
_target_location.alt = packet.relative_alt / 10; // convert millimeters to cm
_target_location.flags.relative_alt = 1; // set relative_alt flag
} else {
// absolute altitude
_target_location.alt = packet.alt / 10; // convert millimeters to cm
_target_location.flags.relative_alt = 0; // reset relative_alt flag
}
_target_velocity_ned.x = packet.vx * 0.01f; // velocity north
_target_velocity_ned.y = packet.vy * 0.01f; // velocity east
_target_velocity_ned.z = packet.vz * 0.01f; // velocity down
_last_location_update_ms = now;
if (packet.hdg <= 36000) { // heading (UINT16_MAX if unknown)
_target_heading = packet.hdg * 0.01f; // convert centi-degrees to degrees
_last_heading_update_ms = now;
}
// initialise _sysid if zero to sender's id
if (_sysid_to_follow == 0) {
_sysid_to_follow = msg.sysid;
}
if ((now - _last_location_sent_to_gcs) > AP_GCS_INTERVAL_MS) {
_last_location_sent_to_gcs = now;
gcs().send_text(MAV_SEVERITY_INFO, "Foll: %u %ld %ld %4.2f\n",
(unsigned)_sysid_to_follow,
(long)_target_location.lat,
(long)_target_location.lng,
(double)(_target_location.alt * 0.01f)); // cm to m
}
// log lead's estimated vs reported position
DataFlash_Class::instance()->Log_Write("FOLL",
"TimeUS,Lat,Lon,Alt,VelN,VelE,VelD,LatE,LonE,AltE", // labels
"sDUmnnnDUm", // units
"F--B000--B", // mults
"QLLifffLLi", // fmt
AP_HAL::micros64(),
_target_location.lat,
_target_location.lng,
_target_location.alt,
(double)_target_velocity_ned.x,
(double)_target_velocity_ned.y,
(double)_target_velocity_ned.z,
loc_estimate.lat,
loc_estimate.lng,
loc_estimate.alt
);
}
}
void Ut_CReporterCoreDir::init()
{
QString mkDir = QString("mkdir -p -m 777 %1 %2").arg(testMountPoint1).arg(testMountPoint2);
UNUSED_RESULT(system(mkDir.toLatin1()));
}
void Ut_CReporterCoreDir::cleanupTestCase()
{
QDir::setCurrent(QDir::homePath());
QString rm = QString("rm -r -f /tmp/crash-reporter-tests");
UNUSED_RESULT(system(rm.toLatin1()));
}
