diff --git a/libraries/AP_Beacon/examples/AP_Marvelmind_test/AP_Marvelmind_test.cpp b/libraries/AP_Beacon/examples/AP_Marvelmind_test/AP_Marvelmind_test.cpp index eca4337a7f..051a7f756a 100644 --- a/libraries/AP_Beacon/examples/AP_Marvelmind_test/AP_Marvelmind_test.cpp +++ b/libraries/AP_Beacon/examples/AP_Marvelmind_test/AP_Marvelmind_test.cpp @@ -11,13 +11,16 @@ void setup(); void loop(); +void set_object_value_and_report(const void *object_pointer, + const struct AP_Param::GroupInfo *group_info, + const char *name, float value); const AP_HAL::HAL& hal = AP_HAL::get_HAL(); static AP_SerialManager serial_manager; AP_Beacon beacon{serial_manager}; // try to set the object value but provide diagnostic if it failed -void set_object_value(const void *object_pointer, +void set_object_value_and_report(const void *object_pointer, const struct AP_Param::GroupInfo *group_info, const char *name, float value) { @@ -29,8 +32,8 @@ void set_object_value(const void *object_pointer, void setup(void) { - set_object_value(&beacon, beacon.var_info, "_TYPE", 2.0f); - set_object_value(&serial_manager, serial_manager.var_info, "0_PROTOCOL", 13.0f); + set_object_value_and_report(&beacon, beacon.var_info, "_TYPE", 2.0f); + set_object_value_and_report(&serial_manager, serial_manager.var_info, "0_PROTOCOL", 13.0f); serial_manager.init(); beacon.init(); } @@ -43,7 +46,7 @@ void loop(void) float accuracy = 0.0f; beacon.get_vehicle_position_ned(pos, accuracy); if (pos.x > 0.001f) { - printf("%f %f %f\n", pos.x, pos.y, pos.z); + printf("%f %f %f\n", static_cast(pos.x), static_cast(pos.y), static_cast(pos.z)); count++; } hal.scheduler->delay(1000);