void send(const hrt_abstime t)
{
struct vehicle_attitude_s att;
struct vehicle_global_position_s pos;
struct actuator_armed_s armed;
struct actuator_controls_s act;
struct airspeed_s airspeed;
bool updated = att_sub->update(&att_time, &att);
updated |= pos_sub->update(&pos_time, &pos);
updated |= armed_sub->update(&armed_time, &armed);
updated |= act_sub->update(&act_time, &act);
updated |= airspeed_sub->update(&airspeed_time, &airspeed);
if (updated) {
float groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e);
uint16_t heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F;
float throttle = armed.armed ? act.control[3] * 100.0f : 0.0f;
mavlink_msg_vfr_hud_send(_channel,
airspeed.true_airspeed_m_s,
groundspeed,
heading,
throttle,
pos.alt,
-pos.vel_d);
}
}
void BlockWaypointGuidance::update(vehicle_global_position_s &pos,
vehicle_attitude_s &att,
position_setpoint_s &missionCmd,
position_setpoint_s &lastMissionCmd)
{
// heading to waypoint
float psiTrack = get_bearing_to_next_waypoint(
(double)pos.lat / (double)1e7d,
(double)pos.lon / (double)1e7d,
missionCmd.lat,
missionCmd.lon);
// cross track
struct crosstrack_error_s xtrackError;
get_distance_to_line(&xtrackError,
(double)pos.lat / (double)1e7d,
(double)pos.lon / (double)1e7d,
lastMissionCmd.lat,
lastMissionCmd.lon,
missionCmd.lat,
missionCmd.lon);
_psiCmd = _wrap_2pi(psiTrack -
_xtYawLimit.update(_xt2Yaw.update(xtrackError.distance)));
}
void send(const hrt_abstime t)
{
if (gps_sub->update(t)) {
mavlink_msg_gps_raw_int_send(_channel,
gps->timestamp_position,
gps->fix_type,
gps->lat,
gps->lon,
gps->alt,
cm_uint16_from_m_float(gps->eph_m),
cm_uint16_from_m_float(gps->epv_m),
gps->vel_m_s * 100.0f,
_wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f,
gps->satellites_visible);
}
}
__EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist,
double *lat_target, double *lon_target)
{
bearing = _wrap_2pi(bearing);
double radius_ratio = (double)(fabs(dist) / CONSTANTS_RADIUS_OF_EARTH);
double lat_start_rad = lat_start * M_DEG_TO_RAD;
double lon_start_rad = lon_start * M_DEG_TO_RAD;
*lat_target = asin(sin(lat_start_rad) * cos(radius_ratio) + cos(lat_start_rad) * sin(radius_ratio) * cos((
double)bearing));
*lon_target = lon_start_rad + atan2(sin((double)bearing) * sin(radius_ratio) * cos(lat_start_rad),
cos(radius_ratio) - sin(lat_start_rad) * sin(*lat_target));
*lat_target *= M_RAD_TO_DEG;
*lon_target *= M_RAD_TO_DEG;
}
__EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist,
double *lat_target, double *lon_target)
{
if (fabsf(dist) < FLT_EPSILON) {
*lat_target = lat_A;
*lon_target = lon_A;
} else if (dist >= FLT_EPSILON) {
float heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B);
waypoint_from_heading_and_distance(lat_A, lon_A, heading, dist, lat_target, lon_target);
} else {
float heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B);
heading = _wrap_2pi(heading + M_PI_F);
waypoint_from_heading_and_distance(lat_A, lon_A, heading, dist, lat_target, lon_target);
}
}
void send(const hrt_abstime t)
{
struct vehicle_gps_position_s gps;
if (gps_sub->update(&gps_time, &gps)) {
mavlink_msg_gps_raw_int_send(_channel,
gps.timestamp_position,
gps.fix_type + (gps.fix_quality << 4),
gps.lat,
gps.lon,
gps.alt,
cm_uint16_from_m_float(gps.eph),
cm_uint16_from_m_float(gps.epv),
gps.vel_m_s * 100.0f,
_wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f,
gps.satellites_used);
}
}
void send(const hrt_abstime t)
{
bool updated = pos_sub->update(t);
updated |= home_sub->update(t);
if (updated) {
mavlink_msg_global_position_int_send(_channel,
pos->timestamp / 1000,
pos->lat * 1e7,
pos->lon * 1e7,
pos->alt * 1000.0f,
(pos->alt - home->alt) * 1000.0f,
pos->vel_n * 100.0f,
pos->vel_e * 100.0f,
pos->vel_d * 100.0f,
_wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f);
}
}
void send(const hrt_abstime t)
{
struct vehicle_global_position_s pos;
struct home_position_s home;
bool updated = pos_sub->update(&pos_time, &pos);
updated |= home_sub->update(&home_time, &home);
if (updated) {
mavlink_msg_global_position_int_send(_channel,
pos.timestamp / 1000,
pos.lat * 1e7,
pos.lon * 1e7,
pos.alt * 1000.0f,
(pos.alt - home.alt) * 1000.0f,
pos.vel_n * 100.0f,
pos.vel_e * 100.0f,
pos.vel_d * 100.0f,
_wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f);
}
}
void send(const hrt_abstime t)
{
bool updated = att_sub->update(t);
updated |= pos_sub->update(t);
updated |= armed_sub->update(t);
updated |= act_sub->update(t);
updated |= airspeed_sub->update(t);
if (updated) {
float groundspeed = sqrtf(pos->vel_n * pos->vel_n + pos->vel_e * pos->vel_e);
uint16_t heading = _wrap_2pi(att->yaw) * M_RAD_TO_DEG_F;
float throttle = armed->armed ? act->control[3] * 100.0f : 0.0f;
mavlink_msg_vfr_hud_send(_channel,
airspeed->true_airspeed_m_s,
groundspeed,
heading,
throttle,
pos->alt,
-pos->vel_d);
}
}
