void
AP_GPS::send_mavlink_gps2_raw(mavlink_channel_t chan)
{
static uint32_t last_send_time_ms2;
if (num_sensors() > 1 &&
status(1) > AP_GPS::NO_GPS &&
(last_send_time_ms2 == 0 || last_send_time_ms2 != last_message_time_ms(1))) {
const Location &loc = location(1);
last_send_time_ms2 = last_message_time_ms(1);
mavlink_msg_gps2_raw_send(
chan,
last_fix_time_ms(1)*(uint64_t)1000,
status(1),
loc.lat,
loc.lng,
loc.alt * 10UL,
get_hdop(1),
65535,
ground_speed(1)*100, // cm/s
ground_course_cd(1), // 1/100 degrees,
num_sats(1),
0,
0);
}
}
void
AP_GPS::send_mavlink_gps_raw(mavlink_channel_t chan)
{
static uint32_t last_send_time_ms[MAVLINK_COMM_NUM_BUFFERS];
if (status(0) > AP_GPS::NO_GPS) {
// when we have a GPS then only send new data
if (last_send_time_ms[chan] == last_message_time_ms(0)) {
return;
}
last_send_time_ms[chan] = last_message_time_ms(0);
} else {
// when we don't have a GPS then send at 1Hz
uint32_t now = AP_HAL::millis();
if (now - last_send_time_ms[chan] < 1000) {
return;
}
last_send_time_ms[chan] = now;
}
const Location &loc = location(0);
mavlink_msg_gps_raw_int_send(
chan,
last_fix_time_ms(0)*(uint64_t)1000,
status(0),
loc.lat, // in 1E7 degrees
loc.lng, // in 1E7 degrees
loc.alt * 10UL, // in mm
get_hdop(0),
get_vdop(0),
ground_speed(0)*100, // cm/s
ground_course_cd(0), // 1/100 degrees,
num_sats(0));
}
void
AP_GPS::send_mavlink_gps2_raw(mavlink_channel_t chan)
{
static uint32_t last_send_time_ms[MAVLINK_COMM_NUM_BUFFERS];
if (num_sensors() < 2 || status(1) <= AP_GPS::NO_GPS) {
return;
}
// when we have a GPS then only send new data
if (last_send_time_ms[chan] == last_message_time_ms(1)) {
return;
}
last_send_time_ms[chan] = last_message_time_ms(1);
const Location &loc = location(1);
mavlink_msg_gps2_raw_send(
chan,
last_fix_time_ms(1)*(uint64_t)1000,
status(1),
loc.lat,
loc.lng,
loc.alt * 10UL,
get_hdop(1),
get_vdop(1),
ground_speed(1)*100, // cm/s
ground_course_cd(1), // 1/100 degrees,
num_sats(1),
0,
0);
}
void
AP_GPS::send_mavlink_gps_raw(mavlink_channel_t chan)
{
static uint32_t last_send_time_ms;
if (last_send_time_ms == 0 || last_send_time_ms != last_message_time_ms(0)) {
last_send_time_ms = last_message_time_ms(0);
const Location &loc = location(0);
mavlink_msg_gps_raw_int_send(
chan,
last_fix_time_ms(0)*(uint64_t)1000,
status(0),
loc.lat, // in 1E7 degrees
loc.lng, // in 1E7 degrees
loc.alt * 10UL, // in mm
get_hdop(0),
65535,
ground_speed(0)*100, // cm/s
ground_course_cd(0), // 1/100 degrees,
num_sats(0));
}
}
void
FixedwingPositionControl::task_main()
{
/* inform about start */
warnx("Initializing..");
fflush(stdout);
/*
* do subscriptions
*/
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder));
/* rate limit vehicle status updates to 5Hz */
orb_set_interval(_control_mode_sub, 200);
/* rate limit position updates to 50 Hz */
orb_set_interval(_global_pos_sub, 20);
/* abort on a nonzero return value from the parameter init */
if (parameters_update()) {
/* parameter setup went wrong, abort */
warnx("aborting startup due to errors.");
_task_should_exit = true;
}
/* wakeup source(s) */
struct pollfd fds[2];
/* Setup of loop */
fds[0].fd = _params_sub;
fds[0].events = POLLIN;
fds[1].fd = _global_pos_sub;
fds[1].events = POLLIN;
while (!_task_should_exit) {
/* wait for up to 500ms for data */
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
/* timed out - periodic check for _task_should_exit, etc. */
if (pret == 0)
continue;
/* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) {
warn("poll error %d, %d", pret, errno);
continue;
}
perf_begin(_loop_perf);
/* check vehicle status for changes to publication state */
vehicle_control_mode_poll();
/* only update parameters if they changed */
if (fds[0].revents & POLLIN) {
/* read from param to clear updated flag */
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), _params_sub, &update);
/* update parameters from storage */
parameters_update();
}
/* only run controller if position changed */
if (fds[1].revents & POLLIN) {
/* XXX Hack to get mavlink output going */
if (_mavlink_fd < 0) {
/* try to open the mavlink log device every once in a while */
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
}
static uint64_t last_run = 0;
float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
last_run = hrt_absolute_time();
/* guard against too large deltaT's */
if (deltaT > 1.0f)
deltaT = 0.01f;
/* load local copies */
orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
// XXX add timestamp check
_global_pos_valid = true;
vehicle_attitude_poll();
vehicle_setpoint_poll();
vehicle_sensor_combined_poll();
vehicle_airspeed_poll();
range_finder_poll();
// vehicle_baro_poll();
math::Vector<2> ground_speed(_global_pos.vel_n, _global_pos.vel_e);
math::Vector<2> current_position((float)_global_pos.lat, (float)_global_pos.lon);
/*
* Attempt to control position, on success (= sensors present and not in manual mode),
* publish setpoint.
*/
if (control_position(current_position, ground_speed, _pos_sp_triplet)) {
_att_sp.timestamp = hrt_absolute_time();
/* lazily publish the setpoint only once available */
if (_attitude_sp_pub > 0) {
/* publish the attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &_att_sp);
} else {
/* advertise and publish */
_attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
}
/* XXX check if radius makes sense here */
float turn_distance = _l1_control.switch_distance(100.0f);
/* lazily publish navigation capabilities */
if (fabsf(turn_distance - _nav_capabilities.turn_distance) > FLT_EPSILON && turn_distance > 0) {
/* set new turn distance */
_nav_capabilities.turn_distance = turn_distance;
navigation_capabilities_publish();
}
}
}
perf_end(_loop_perf);
}
warnx("exiting.\n");
_control_task = -1;
_exit(0);
}