diff --git a/libraries/SITL/SIM_FETtecOneWireESC.cpp b/libraries/SITL/SIM_FETtecOneWireESC.cpp index c1159a4352..4af2bdb33d 100644 --- a/libraries/SITL/SIM_FETtecOneWireESC.cpp +++ b/libraries/SITL/SIM_FETtecOneWireESC.cpp @@ -37,6 +37,10 @@ Protocol: - in the case that we don't have ESC telemetry, consider probing ESCs periodically with an "OK"-request while disarmed */ +#include + +extern const AP_HAL::HAL& hal; + #include #include "SIM_FETtecOneWireESC.h" @@ -227,7 +231,7 @@ void FETtecOneWireESC::running_handle_config_message(FETtecOneWireESC::ESC &esc) case ConfigMessageType::NOT_OK: break; case ConfigMessageType::BL_START_FW: // BL only - ::fprintf(stderr, "received unexpected BL_START_FW message\n"); + hal.console->printf("received unexpected BL_START_FW message\n"); AP_HAL::panic("received unexpected BL_START_FW message"); return; case ConfigMessageType::BL_PAGES_TO_FLASH: // BL only diff --git a/libraries/SITL/SIM_Frsky_D.cpp b/libraries/SITL/SIM_Frsky_D.cpp index 754b31bd84..f219b182f8 100644 --- a/libraries/SITL/SIM_Frsky_D.cpp +++ b/libraries/SITL/SIM_Frsky_D.cpp @@ -18,7 +18,9 @@ #include "SIM_Frsky_D.h" -#include +#include + +extern const AP_HAL::HAL& hal; using namespace SITL; @@ -30,7 +32,7 @@ static const uint8_t BYTESTUFF_D = 0x5D; void Frsky_D::handle_data(uint8_t id, uint16_t data) { - ::fprintf(stderr, + hal.console->printf( "Frsky: id=%s (0x%02X) data=%u\n", dataid_string((DataID)id), (unsigned)_id, diff --git a/libraries/SITL/SIM_Gripper_EPM.cpp b/libraries/SITL/SIM_Gripper_EPM.cpp index 4417a32f92..09f73102a4 100644 --- a/libraries/SITL/SIM_Gripper_EPM.cpp +++ b/libraries/SITL/SIM_Gripper_EPM.cpp @@ -21,6 +21,8 @@ #include #include +extern const AP_HAL::HAL& hal; + using namespace SITL; // table of user settable parameters @@ -81,9 +83,9 @@ void Gripper_EPM::update_from_demand() } if (should_report()) { - ::fprintf(stderr, "demand=%f\n", demand); - printf("Field strength: %f%%\n", field_strength); - printf("Field strength: %f Tesla\n", tesla()); + hal.console->printf("demand=%f\n", demand); + hal.console->printf("Field strength: %f%%\n", field_strength); + hal.console->printf("Field strength: %f Tesla\n", tesla()); last_report_us = now; reported_field_strength = field_strength; } diff --git a/libraries/SITL/SIM_Gripper_Servo.cpp b/libraries/SITL/SIM_Gripper_Servo.cpp index e563cd2727..2237e69a12 100644 --- a/libraries/SITL/SIM_Gripper_Servo.cpp +++ b/libraries/SITL/SIM_Gripper_Servo.cpp @@ -21,6 +21,8 @@ #include "AP_Math/AP_Math.h" #include +extern const AP_HAL::HAL& hal; + using namespace SITL; // table of user settable parameters @@ -96,7 +98,7 @@ void Gripper_Servo::update(const struct sitl_input &input) jaw_gap = gap * (1.0f - position); } if (should_report()) { - ::fprintf(stderr, "position_demand=%f jaw_gap=%f load=%f\n", position_demand, jaw_gap, load_mass); + hal.console->printf("position_demand=%f jaw_gap=%f load=%f\n", position_demand, jaw_gap, load_mass); last_report_us = now; reported_position = position; } diff --git a/libraries/SITL/SIM_MS5XXX.cpp b/libraries/SITL/SIM_MS5XXX.cpp index 88c1e6f76e..053a832048 100644 --- a/libraries/SITL/SIM_MS5XXX.cpp +++ b/libraries/SITL/SIM_MS5XXX.cpp @@ -6,6 +6,9 @@ using namespace SITL; +#include +extern const AP_HAL::HAL& hal; + MS5XXX::MS5XXX() : I2CDevice() { @@ -163,7 +166,7 @@ int MS5XXX::rdwr(I2C::i2c_rdwr_ioctl_data *&data) cmd == Command::RESET) { // this is OK - RESET is OK in UNINITIALISED } else { - ::fprintf(stderr, "Command (0x%02x) received while not running (state=%u)\n", (unsigned)cmd, (unsigned)state); + hal.console->printf("Command (0x%02x) received while not running (state=%u)\n", (unsigned)cmd, (unsigned)state); return -1; // we could die instead... } } @@ -199,7 +202,7 @@ int MS5XXX::rdwr(I2C::i2c_rdwr_ioctl_data *&data) if (data->msgs[1].len == 0) { // upon not getting a reading back the driver commands a // conversion-read but doesn't wait for a response! - ::fprintf(stderr, "read of length zero\n"); + hal.console->printf("read of length zero\n"); return -1; } if (data->msgs[1].len != 3) { diff --git a/libraries/SITL/SIM_Vicon.cpp b/libraries/SITL/SIM_Vicon.cpp index 0187e0ae45..07c24f4592 100644 --- a/libraries/SITL/SIM_Vicon.cpp +++ b/libraries/SITL/SIM_Vicon.cpp @@ -24,6 +24,8 @@ #include #include +extern const AP_HAL::HAL& hal; + using namespace SITL; Vicon::Vicon() : @@ -89,7 +91,7 @@ void Vicon::update_vicon_position_estimate(const Location &loc, uint16_t buf_len = mavlink_msg_to_send_buffer(buf, &msg_buf[i].obs_msg); if (::write(fd_my_end, (void*)&buf, buf_len) != buf_len) { - ::fprintf(stderr, "Vicon: write failure\n"); + hal.console->printf("Vicon: write failure\n"); } msg_buf[i].time_send_us = 0; }