・It is connected to GPS1 of Cube+
・The writing function in Log.cpp(The part that handles the writing process of the defined logs and the detailed settings of the logs to be left behind) of c++ is as follows
// Write a sonar packet
void Plane::Log_Write_Sonar()
{
#if RANGEFINDER_ENABLED == ENABLED
uint16_t distance = 0;
if (rangefinder.status() == RangeFinder::RangeFinder_Good) {
distance = rangefinder.distance_cm();
}
struct log_Sonar pkt = {
LOG_PACKET_HEADER_INIT(LOG_SONAR_MSG),
time_us : AP_HAL::micros64(),
distance : (float)distance*0.01f
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
DataFlash.Log_Write_RFND(rangefinder);
#endif
}
・The part described in commands_logic.cpp
/*
update navigation for normal mission waypoints. Return true when the waypoint is complete /
bool Plane::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
{
steer_state.hold_course_cd = -1;
// depending on the pass by flag either go to waypoint in regular manner or
// fly past it for set distance along the line of waypoints
Location flex_next_WP_loc = next_WP_loc;
uint8_t cmd_passby = HIGHBYTE(cmd.p1); // distance in meters to pass beyond the wp
uint8_t cmd_acceptance_distance = LOWBYTE(cmd.p1); // radius in meters to accept reaching the wp
if (cmd_passby > 0) {
float dist = prev_WP_loc.get_distance(flex_next_WP_loc);
if (!is_zero(dist)) {
float factor = (dist + cmd_passby) / dist;
flex_next_WP_loc.lat = flex_next_WP_loc.lat + (flex_next_WP_loc.lat - prev_WP_loc.lat)* (factor - 1.0f);
flex_next_WP_loc.lng = flex_next_WP_loc.lng +
Location::diff_longitude(flex_next_WP_loc.lng,prev_WP_loc.lng) * (factor - 1.0f);
}
}
if (auto_state.crosstrack) {
nav_controller->update_waypoint(prev_WP_loc, flex_next_WP_loc);
} else {
nav_controller->update_waypoint(current_loc, flex_next_WP_loc);
}
・The error message is as follows
uint16_t distance=0 → Unused variable ‘distance’
Float dist = prev_WP_loc.get_distance(flex_next_WP_loc); ->declaration of ‘dist’ shadows a member of ‘Plane’