void estimator_base::gps_poll() { _gps_new = false; orb_check(_gps_sub, &_gps_new); if (_gps_new) { orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps); } if (_gps.fix_type < 3 || !isfinite(_gps.lon) || !isfinite(_gps.lat) || !isfinite(_gps.alt) ) { _gps_new = false; } if (_gps_new && !_gps_init) { _gps_init = true; _init_lon = _gps.lon; _init_lat = _gps.lat; _init_alt = _gps.alt; } }
void MulticopterAttitudeControl::vehicle_status_poll() { /* check if there is new status information */ bool vehicle_status_updated; orb_check(_vehicle_status_sub, &vehicle_status_updated); if (vehicle_status_updated) { orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); /* set correct uORB ID, depending on if vehicle is VTOL or not */ if (!_rates_sp_id) { if (_vehicle_status.is_vtol) { _rates_sp_id = ORB_ID(mc_virtual_rates_setpoint); _actuators_id = ORB_ID(actuator_controls_virtual_mc); } else { _rates_sp_id = ORB_ID(vehicle_rates_setpoint); _actuators_id = ORB_ID(actuator_controls_0); } } } }
void FixedwingPositionControl::vehicle_control_mode_poll() { bool vstatus_updated; /* Check HIL state if vehicle status has changed */ orb_check(_control_mode_sub, &vstatus_updated); if (vstatus_updated) { bool was_armed = _control_mode.flag_armed; orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); if (!was_armed && _control_mode.flag_armed) { _launch_lat = _global_pos.lat; _launch_lon = _global_pos.lon; _launch_alt = _global_pos.alt; _launch_valid = true; } } }
void MulticopterLandDetector::updateParameterCache(const bool force) { bool updated; parameter_update_s paramUpdate; orb_check(_parameterSub, &updated); if (updated) { orb_copy(ORB_ID(parameter_update), _parameterSub, ¶mUpdate); } if (updated || force) { param_get(_paramHandle.maxClimbRate, &_params.maxClimbRate); param_get(_paramHandle.maxVelocity, &_params.maxVelocity); param_get(_paramHandle.maxRotation, &_params.maxRotation_rad_s); _params.maxRotation_rad_s = math::radians(_params.maxRotation_rad_s); param_get(_paramHandle.maxThrottle, &_params.maxThrottle); param_get(_paramHandle.minManThrottle, &_params.minManThrottle); param_get(_paramHandle.acc_threshold_m_s2, &_params.acc_threshold_m_s2); param_get(_paramHandle.ff_trigger_time, &_params.ff_trigger_time); } }
void OutputBase::_handle_position_update(bool force_update) { bool need_update = force_update; if (!_cur_control_data || _cur_control_data->type != ControlData::Type::LonLat) { return; } if (!force_update) { orb_check(_vehicle_global_position_sub, &need_update); } if (!need_update) { return; } vehicle_global_position_s vehicle_global_position; orb_copy(ORB_ID(vehicle_global_position), _vehicle_global_position_sub, &vehicle_global_position); float pitch; const double &lon = _cur_control_data->type_data.lonlat.lon; const double &lat = _cur_control_data->type_data.lonlat.lat; const float &alt = _cur_control_data->type_data.lonlat.altitude; if (_cur_control_data->type_data.lonlat.pitch_fixed_angle >= -M_PI_F) { pitch = _cur_control_data->type_data.lonlat.pitch_fixed_angle; } else { pitch = _calculate_pitch(lon, lat, alt, vehicle_global_position); } float roll = _cur_control_data->type_data.lonlat.roll_angle; float yaw = get_bearing_to_next_waypoint(vehicle_global_position.lat, vehicle_global_position.lon, lat, lon) - vehicle_global_position.yaw; _angle_setpoints[0] = roll; _angle_setpoints[1] = pitch + _cur_control_data->type_data.lonlat.pitch_angle_offset; _angle_setpoints[2] = yaw + _cur_control_data->type_data.lonlat.yaw_angle_offset; }
bool MavlinkOrbSubscription::is_published() { // If we marked it as published no need to check again if (_published) { return true; } hrt_abstime now = hrt_absolute_time(); if (now - _last_pub_check < 300000) { return false; } // We are checking now _last_pub_check = now; // We don't want to subscribe to anything that does not exist // in order to save memory and file descriptors. // However, for some topics like vehicle_command_ack, we want to subscribe // from the beginning in order not to miss or delay the first publish respective advertise. if (!_subscribe_from_beginning && orb_exists(_topic, _instance)) { return false; } if (_fd < 0) { _fd = orb_subscribe_multi(_topic, _instance); } bool updated; orb_check(_fd, &updated); if (updated) { _published = true; } return _published; }
bool MavlinkOrbSubscription::update_if_changed(void *data) { bool prevpub = _published; if (!is_published()) { return false; } bool updated; if (orb_check(_fd, &updated)) { return false; } // If we didn't update and this topic did not change // its publication status then nothing really changed if (!updated && prevpub == _published) { return false; } return update(data); }
void GPS::handleInjectDataTopic() { if (_orb_inject_data_fd[0] == -1) { return; } bool updated = false; do { int orb_inject_data_cur_fd = _orb_inject_data_fd[_orb_inject_data_next]; orb_check(orb_inject_data_cur_fd, &updated); if (updated) { struct gps_inject_data_s msg; orb_copy(ORB_ID(gps_inject_data), orb_inject_data_cur_fd, &msg); injectData(msg.data, msg.len); _orb_inject_data_next = (_orb_inject_data_next + 1) % _orb_inject_data_fd_count; ++_last_rate_rtcm_injection_count; } } while (updated); }
int do_trim_calibration(int mavlink_fd) { int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint)); usleep(200000); struct manual_control_setpoint_s sp; bool changed; orb_check(sub_man, &changed); if (!changed) { mavlink_log_critical(mavlink_fd, "no inputs, aborting"); return ERROR; } orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp); /* set parameters */ float p = sp.roll; param_set(param_find("TRIM_ROLL"), &p); p = sp.pitch; param_set(param_find("TRIM_PITCH"), &p); p = sp.yaw; param_set(param_find("TRIM_YAW"), &p); /* store to permanent storage */ /* auto-save */ int save_ret = param_save_default(); if (save_ret != 0) { mavlink_log_critical(mavlink_fd, "TRIM: SAVE FAIL"); close(sub_man); return ERROR; } mavlink_log_info(mavlink_fd, "trim cal done"); close(sub_man); return OK; }
void MulticopterPositionControl::poll_subscriptions() { bool updated; orb_check(_att_sub, &updated); if (updated) { orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); } orb_check(_att_sp_sub, &updated); if (updated) { orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp); } orb_check(_control_mode_sub, &updated); if (updated) { orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); } orb_check(_manual_sub, &updated); if (updated) { orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual); } orb_check(_arming_sub, &updated); if (updated) { orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming); } orb_check(_local_pos_sub, &updated); if (updated) { orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos); } }
void Ekf2::task_main() { // subscribe to relevant topics int sensors_sub = orb_subscribe(ORB_ID(sensor_combined)); int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); int airspeed_sub = orb_subscribe(ORB_ID(airspeed)); int params_sub = orb_subscribe(ORB_ID(parameter_update)); int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow)); int range_finder_sub = orb_subscribe(ORB_ID(distance_sensor)); int ev_pos_sub = orb_subscribe(ORB_ID(vehicle_vision_position)); int ev_att_sub = orb_subscribe(ORB_ID(vehicle_vision_attitude)); int vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); int status_sub = orb_subscribe(ORB_ID(vehicle_status)); px4_pollfd_struct_t fds[2] = {}; fds[0].fd = sensors_sub; fds[0].events = POLLIN; fds[1].fd = params_sub; fds[1].events = POLLIN; // initialise parameter cache updateParams(); // initialize data structures outside of loop // because they will else not always be // properly populated sensor_combined_s sensors = {}; vehicle_gps_position_s gps = {}; airspeed_s airspeed = {}; optical_flow_s optical_flow = {}; distance_sensor_s range_finder = {}; vehicle_land_detected_s vehicle_land_detected = {}; vehicle_local_position_s ev_pos = {}; vehicle_attitude_s ev_att = {}; vehicle_status_s vehicle_status = {}; while (!_task_should_exit) { int ret = px4_poll(fds, sizeof(fds) / sizeof(fds[0]), 1000); if (ret < 0) { // Poll error, sleep and try again usleep(10000); continue; } else if (ret == 0) { // Poll timeout or no new data, do nothing continue; } if (fds[1].revents & POLLIN) { // read from param to clear updated flag struct parameter_update_s update; orb_copy(ORB_ID(parameter_update), params_sub, &update); updateParams(); // fetch sensor data in next loop continue; } else if (!(fds[0].revents & POLLIN)) { // no new data continue; } bool gps_updated = false; bool airspeed_updated = false; bool optical_flow_updated = false; bool range_finder_updated = false; bool vehicle_land_detected_updated = false; bool vision_position_updated = false; bool vision_attitude_updated = false; bool vehicle_status_updated = false; orb_copy(ORB_ID(sensor_combined), sensors_sub, &sensors); // update all other topics if they have new data orb_check(status_sub, &vehicle_status_updated); if (vehicle_status_updated) { orb_copy(ORB_ID(vehicle_status), status_sub, &vehicle_status); } orb_check(gps_sub, &gps_updated); if (gps_updated) { orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps); } orb_check(airspeed_sub, &airspeed_updated); if (airspeed_updated) { orb_copy(ORB_ID(airspeed), airspeed_sub, &airspeed); } orb_check(optical_flow_sub, &optical_flow_updated); if (optical_flow_updated) { orb_copy(ORB_ID(optical_flow), optical_flow_sub, &optical_flow); } orb_check(range_finder_sub, &range_finder_updated); if (range_finder_updated) { orb_copy(ORB_ID(distance_sensor), range_finder_sub, &range_finder); if (range_finder.min_distance > range_finder.current_distance || range_finder.max_distance < range_finder.current_distance) { range_finder_updated = false; } } orb_check(ev_pos_sub, &vision_position_updated); if (vision_position_updated) { orb_copy(ORB_ID(vehicle_vision_position), ev_pos_sub, &ev_pos); } orb_check(ev_att_sub, &vision_attitude_updated); if (vision_attitude_updated) { orb_copy(ORB_ID(vehicle_vision_attitude), ev_att_sub, &ev_att); } // in replay mode we are getting the actual timestamp from the sensor topic hrt_abstime now = 0; if (_replay_mode) { now = sensors.timestamp; } else { now = hrt_absolute_time(); } // push imu data into estimator float gyro_integral[3]; gyro_integral[0] = sensors.gyro_rad[0] * sensors.gyro_integral_dt; gyro_integral[1] = sensors.gyro_rad[1] * sensors.gyro_integral_dt; gyro_integral[2] = sensors.gyro_rad[2] * sensors.gyro_integral_dt; float accel_integral[3]; accel_integral[0] = sensors.accelerometer_m_s2[0] * sensors.accelerometer_integral_dt; accel_integral[1] = sensors.accelerometer_m_s2[1] * sensors.accelerometer_integral_dt; accel_integral[2] = sensors.accelerometer_m_s2[2] * sensors.accelerometer_integral_dt; _ekf.setIMUData(now, sensors.gyro_integral_dt * 1.e6f, sensors.accelerometer_integral_dt * 1.e6f, gyro_integral, accel_integral); // read mag data if (sensors.magnetometer_timestamp_relative == sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) { // set a zero timestamp to let the ekf replay program know that this data is not valid _timestamp_mag_us = 0; } else { if ((sensors.timestamp + sensors.magnetometer_timestamp_relative) != _timestamp_mag_us) { _timestamp_mag_us = sensors.timestamp + sensors.magnetometer_timestamp_relative; // If the time last used by the EKF is less than specified, then accumulate the // data and push the average when the 50msec is reached. _mag_time_sum_ms += _timestamp_mag_us / 1000; _mag_sample_count++; _mag_data_sum[0] += sensors.magnetometer_ga[0]; _mag_data_sum[1] += sensors.magnetometer_ga[1]; _mag_data_sum[2] += sensors.magnetometer_ga[2]; uint32_t mag_time_ms = _mag_time_sum_ms / _mag_sample_count; if (mag_time_ms - _mag_time_ms_last_used > _params->sensor_interval_min_ms) { float mag_sample_count_inv = 1.0f / (float)_mag_sample_count; float mag_data_avg_ga[3] = {_mag_data_sum[0] *mag_sample_count_inv, _mag_data_sum[1] *mag_sample_count_inv, _mag_data_sum[2] *mag_sample_count_inv}; _ekf.setMagData(1000 * (uint64_t)mag_time_ms, mag_data_avg_ga); _mag_time_ms_last_used = mag_time_ms; _mag_time_sum_ms = 0; _mag_sample_count = 0; _mag_data_sum[0] = 0.0f; _mag_data_sum[1] = 0.0f; _mag_data_sum[2] = 0.0f; } } } // read baro data if (sensors.baro_timestamp_relative == sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) { // set a zero timestamp to let the ekf replay program know that this data is not valid _timestamp_balt_us = 0; } else { if ((sensors.timestamp + sensors.baro_timestamp_relative) != _timestamp_balt_us) { _timestamp_balt_us = sensors.timestamp + sensors.baro_timestamp_relative; // If the time last used by the EKF is less than specified, then accumulate the // data and push the average when the 50msec is reached. _balt_time_sum_ms += _timestamp_balt_us / 1000; _balt_sample_count++; _balt_data_sum += sensors.baro_alt_meter; uint32_t balt_time_ms = _balt_time_sum_ms / _balt_sample_count; if (balt_time_ms - _balt_time_ms_last_used > (uint32_t)_params->sensor_interval_min_ms) { float balt_data_avg = _balt_data_sum / (float)_balt_sample_count; _ekf.setBaroData(1000 * (uint64_t)balt_time_ms, balt_data_avg); _balt_time_ms_last_used = balt_time_ms; _balt_time_sum_ms = 0; _balt_sample_count = 0; _balt_data_sum = 0.0f; } } } // read gps data if available if (gps_updated) { struct gps_message gps_msg = {}; gps_msg.time_usec = gps.timestamp; gps_msg.lat = gps.lat; gps_msg.lon = gps.lon; gps_msg.alt = gps.alt; gps_msg.fix_type = gps.fix_type; gps_msg.eph = gps.eph; gps_msg.epv = gps.epv; gps_msg.sacc = gps.s_variance_m_s; gps_msg.vel_m_s = gps.vel_m_s; gps_msg.vel_ned[0] = gps.vel_n_m_s; gps_msg.vel_ned[1] = gps.vel_e_m_s; gps_msg.vel_ned[2] = gps.vel_d_m_s; gps_msg.vel_ned_valid = gps.vel_ned_valid; gps_msg.nsats = gps.satellites_used; //TODO add gdop to gps topic gps_msg.gdop = 0.0f; _ekf.setGpsData(gps.timestamp, &gps_msg); } // only set airspeed data if condition for airspeed fusion are met bool fuse_airspeed = airspeed_updated && !vehicle_status.is_rotary_wing && _arspFusionThreshold.get() <= airspeed.true_airspeed_m_s && _arspFusionThreshold.get() >= 0.1f; if (fuse_airspeed) { float eas2tas = airspeed.true_airspeed_m_s / airspeed.indicated_airspeed_m_s; _ekf.setAirspeedData(airspeed.timestamp, airspeed.true_airspeed_m_s, eas2tas); } // only fuse synthetic sideslip measurements if conditions are met bool fuse_beta = !vehicle_status.is_rotary_wing && _fuseBeta.get(); _ekf.set_fuse_beta_flag(fuse_beta); if (optical_flow_updated) { flow_message flow; flow.flowdata(0) = optical_flow.pixel_flow_x_integral; flow.flowdata(1) = optical_flow.pixel_flow_y_integral; flow.quality = optical_flow.quality; flow.gyrodata(0) = optical_flow.gyro_x_rate_integral; flow.gyrodata(1) = optical_flow.gyro_y_rate_integral; flow.gyrodata(2) = optical_flow.gyro_z_rate_integral; flow.dt = optical_flow.integration_timespan; if (PX4_ISFINITE(optical_flow.pixel_flow_y_integral) && PX4_ISFINITE(optical_flow.pixel_flow_x_integral)) { _ekf.setOpticalFlowData(optical_flow.timestamp, &flow); } } if (range_finder_updated) { _ekf.setRangeData(range_finder.timestamp, range_finder.current_distance); } // get external vision data // if error estimates are unavailable, use parameter defined defaults if (vision_position_updated || vision_attitude_updated) { ext_vision_message ev_data; ev_data.posNED(0) = ev_pos.x; ev_data.posNED(1) = ev_pos.y; ev_data.posNED(2) = ev_pos.z; Quaternion q(ev_att.q); ev_data.quat = q; // position measurement error from parameters. TODO : use covariances from topic ev_data.posErr = _default_ev_pos_noise; ev_data.angErr = _default_ev_ang_noise; // use timestamp from external computer, clocks are synchronized when using MAVROS _ekf.setExtVisionData(vision_position_updated ? ev_pos.timestamp : ev_att.timestamp, &ev_data); } orb_check(vehicle_land_detected_sub, &vehicle_land_detected_updated); if (vehicle_land_detected_updated) { orb_copy(ORB_ID(vehicle_land_detected), vehicle_land_detected_sub, &vehicle_land_detected); _ekf.set_in_air_status(!vehicle_land_detected.landed); } // run the EKF update and output if (_ekf.update()) { matrix::Quaternion<float> q; _ekf.copy_quaternion(q.data()); float velocity[3]; _ekf.get_velocity(velocity); float gyro_rad[3]; { // generate control state data control_state_s ctrl_state = {}; float gyro_bias[3] = {}; _ekf.get_gyro_bias(gyro_bias); ctrl_state.timestamp = _replay_mode ? now : hrt_absolute_time(); gyro_rad[0] = sensors.gyro_rad[0] - gyro_bias[0]; gyro_rad[1] = sensors.gyro_rad[1] - gyro_bias[1]; gyro_rad[2] = sensors.gyro_rad[2] - gyro_bias[2]; ctrl_state.roll_rate = _lp_roll_rate.apply(gyro_rad[0]); ctrl_state.pitch_rate = _lp_pitch_rate.apply(gyro_rad[1]); ctrl_state.yaw_rate = _lp_yaw_rate.apply(gyro_rad[2]); ctrl_state.roll_rate_bias = gyro_bias[0]; ctrl_state.pitch_rate_bias = gyro_bias[1]; ctrl_state.yaw_rate_bias = gyro_bias[2]; // Velocity in body frame Vector3f v_n(velocity); matrix::Dcm<float> R_to_body(q.inversed()); Vector3f v_b = R_to_body * v_n; ctrl_state.x_vel = v_b(0); ctrl_state.y_vel = v_b(1); ctrl_state.z_vel = v_b(2); // Local Position NED float position[3]; _ekf.get_position(position); ctrl_state.x_pos = position[0]; ctrl_state.y_pos = position[1]; ctrl_state.z_pos = position[2]; // Attitude quaternion q.copyTo(ctrl_state.q); _ekf.get_quat_reset(&ctrl_state.delta_q_reset[0], &ctrl_state.quat_reset_counter); // Acceleration data matrix::Vector<float, 3> acceleration(sensors.accelerometer_m_s2); float accel_bias[3]; _ekf.get_accel_bias(accel_bias); ctrl_state.x_acc = acceleration(0) - accel_bias[0]; ctrl_state.y_acc = acceleration(1) - accel_bias[1]; ctrl_state.z_acc = acceleration(2) - accel_bias[2]; // compute lowpass filtered horizontal acceleration acceleration = R_to_body.transpose() * acceleration; _acc_hor_filt = 0.95f * _acc_hor_filt + 0.05f * sqrtf(acceleration(0) * acceleration(0) + acceleration(1) * acceleration(1)); ctrl_state.horz_acc_mag = _acc_hor_filt; ctrl_state.airspeed_valid = false; // use estimated velocity for airspeed estimate if (_airspeed_mode.get() == control_state_s::AIRSPD_MODE_MEAS) { // use measured airspeed if (PX4_ISFINITE(airspeed.indicated_airspeed_m_s) && hrt_absolute_time() - airspeed.timestamp < 1e6 && airspeed.timestamp > 0) { ctrl_state.airspeed = airspeed.indicated_airspeed_m_s; ctrl_state.airspeed_valid = true; } } else if (_airspeed_mode.get() == control_state_s::AIRSPD_MODE_EST) { if (_ekf.local_position_is_valid()) { ctrl_state.airspeed = sqrtf(velocity[0] * velocity[0] + velocity[1] * velocity[1] + velocity[2] * velocity[2]); ctrl_state.airspeed_valid = true; } } else if (_airspeed_mode.get() == control_state_s::AIRSPD_MODE_DISABLED) { // do nothing, airspeed has been declared as non-valid above, controllers // will handle this assuming always trim airspeed } // publish control state data if (_control_state_pub == nullptr) { _control_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state); } else { orb_publish(ORB_ID(control_state), _control_state_pub, &ctrl_state); } } { // generate vehicle attitude quaternion data struct vehicle_attitude_s att = {}; att.timestamp = _replay_mode ? now : hrt_absolute_time(); q.copyTo(att.q); att.rollspeed = gyro_rad[0]; att.pitchspeed = gyro_rad[1]; att.yawspeed = gyro_rad[2]; // publish vehicle attitude data if (_att_pub == nullptr) { _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att); } else { orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att); } } // generate vehicle local position data struct vehicle_local_position_s lpos = {}; float pos[3] = {}; lpos.timestamp = _replay_mode ? now : hrt_absolute_time(); // Position of body origin in local NED frame _ekf.get_position(pos); lpos.x = (_ekf.local_position_is_valid()) ? pos[0] : 0.0f; lpos.y = (_ekf.local_position_is_valid()) ? pos[1] : 0.0f; lpos.z = pos[2]; // Velocity of body origin in local NED frame (m/s) lpos.vx = velocity[0]; lpos.vy = velocity[1]; lpos.vz = velocity[2]; // TODO: better status reporting lpos.xy_valid = _ekf.local_position_is_valid(); lpos.z_valid = true; lpos.v_xy_valid = _ekf.local_position_is_valid(); lpos.v_z_valid = true; // Position of local NED origin in GPS / WGS84 frame struct map_projection_reference_s ekf_origin = {}; // true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) _ekf.get_ekf_origin(&lpos.ref_timestamp, &ekf_origin, &lpos.ref_alt); lpos.xy_global = _ekf.global_position_is_valid(); lpos.z_global = true; // true if z is valid and has valid global reference (ref_alt) lpos.ref_lat = ekf_origin.lat_rad * 180.0 / M_PI; // Reference point latitude in degrees lpos.ref_lon = ekf_origin.lon_rad * 180.0 / M_PI; // Reference point longitude in degrees // The rotation of the tangent plane vs. geographical north matrix::Eulerf euler(q); lpos.yaw = euler.psi(); float terrain_vpos; lpos.dist_bottom_valid = _ekf.get_terrain_vert_pos(&terrain_vpos); lpos.dist_bottom = terrain_vpos - pos[2]; // Distance to bottom surface (ground) in meters lpos.dist_bottom_rate = -velocity[2]; // Distance to bottom surface (ground) change rate lpos.surface_bottom_timestamp = hrt_absolute_time(); // Time when new bottom surface found // TODO: uORB definition does not define what these variables are. We have assumed them to be horizontal and vertical 1-std dev accuracy in metres Vector3f pos_var, vel_var; _ekf.get_pos_var(pos_var); _ekf.get_vel_var(vel_var); lpos.eph = sqrtf(pos_var(0) + pos_var(1)); lpos.epv = sqrtf(pos_var(2)); // get state reset information of position and velocity _ekf.get_posD_reset(&lpos.delta_z, &lpos.z_reset_counter); _ekf.get_velD_reset(&lpos.delta_vz, &lpos.vz_reset_counter); _ekf.get_posNE_reset(&lpos.delta_xy[0], &lpos.xy_reset_counter); _ekf.get_velNE_reset(&lpos.delta_vxy[0], &lpos.vxy_reset_counter); // publish vehicle local position data if (_lpos_pub == nullptr) { _lpos_pub = orb_advertise(ORB_ID(vehicle_local_position), &lpos); } else { orb_publish(ORB_ID(vehicle_local_position), _lpos_pub, &lpos); } if (_ekf.global_position_is_valid()) { // generate and publish global position data struct vehicle_global_position_s global_pos = {}; global_pos.timestamp = _replay_mode ? now : hrt_absolute_time(); global_pos.time_utc_usec = gps.time_utc_usec; // GPS UTC timestamp in microseconds double est_lat, est_lon, lat_pre_reset, lon_pre_reset; map_projection_reproject(&ekf_origin, lpos.x, lpos.y, &est_lat, &est_lon); global_pos.lat = est_lat; // Latitude in degrees global_pos.lon = est_lon; // Longitude in degrees map_projection_reproject(&ekf_origin, lpos.x - lpos.delta_xy[0], lpos.y - lpos.delta_xy[1], &lat_pre_reset, &lon_pre_reset); global_pos.delta_lat_lon[0] = est_lat - lat_pre_reset; global_pos.delta_lat_lon[1] = est_lon - lon_pre_reset; global_pos.lat_lon_reset_counter = lpos.xy_reset_counter; global_pos.alt = -pos[2] + lpos.ref_alt; // Altitude AMSL in meters _ekf.get_posD_reset(&global_pos.delta_alt, &global_pos.alt_reset_counter); // global altitude has opposite sign of local down position global_pos.delta_alt *= -1.0f; global_pos.vel_n = velocity[0]; // Ground north velocity, m/s global_pos.vel_e = velocity[1]; // Ground east velocity, m/s global_pos.vel_d = velocity[2]; // Ground downside velocity, m/s global_pos.yaw = euler.psi(); // Yaw in radians -PI..+PI. global_pos.eph = sqrtf(pos_var(0) + pos_var(1));; // Standard deviation of position estimate horizontally global_pos.epv = sqrtf(pos_var(2)); // Standard deviation of position vertically if (lpos.dist_bottom_valid) { global_pos.terrain_alt = lpos.ref_alt - terrain_vpos; // Terrain altitude in m, WGS84 global_pos.terrain_alt_valid = true; // Terrain altitude estimate is valid } else { global_pos.terrain_alt = 0.0f; // Terrain altitude in m, WGS84 global_pos.terrain_alt_valid = false; // Terrain altitude estimate is valid } // TODO use innovatun consistency check timouts to set this global_pos.dead_reckoning = false; // True if this position is estimated through dead-reckoning global_pos.pressure_alt = sensors.baro_alt_meter; // Pressure altitude AMSL (m) if (_vehicle_global_position_pub == nullptr) { _vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); } else { orb_publish(ORB_ID(vehicle_global_position), _vehicle_global_position_pub, &global_pos); } } } else if (_replay_mode) { // in replay mode we have to tell the replay module not to wait for an update // we do this by publishing an attitude with zero timestamp struct vehicle_attitude_s att = {}; att.timestamp = now; if (_att_pub == nullptr) { _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att); } else { orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att); } } // publish estimator status struct estimator_status_s status = {}; status.timestamp = _replay_mode ? now : hrt_absolute_time(); _ekf.get_state_delayed(status.states); _ekf.get_covariances(status.covariances); _ekf.get_gps_check_status(&status.gps_check_fail_flags); _ekf.get_control_mode(&status.control_mode_flags); _ekf.get_filter_fault_status(&status.filter_fault_flags); _ekf.get_innovation_test_status(&status.innovation_check_flags, &status.mag_test_ratio, &status.vel_test_ratio, &status.pos_test_ratio, &status.hgt_test_ratio, &status.tas_test_ratio, &status.hagl_test_ratio); bool dead_reckoning; _ekf.get_ekf_lpos_accuracy(&status.pos_horiz_accuracy, &status.pos_vert_accuracy, &dead_reckoning); _ekf.get_ekf_soln_status(&status.solution_status_flags); _ekf.get_imu_vibe_metrics(status.vibe); if (_estimator_status_pub == nullptr) { _estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &status); } else { orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &status); } // Publish wind estimate struct wind_estimate_s wind_estimate = {}; wind_estimate.timestamp = _replay_mode ? now : hrt_absolute_time(); wind_estimate.windspeed_north = status.states[22]; wind_estimate.windspeed_east = status.states[23]; wind_estimate.covariance_north = status.covariances[22]; wind_estimate.covariance_east = status.covariances[23]; if (_wind_pub == nullptr) { _wind_pub = orb_advertise(ORB_ID(wind_estimate), &wind_estimate); } else { orb_publish(ORB_ID(wind_estimate), _wind_pub, &wind_estimate); } // publish estimator innovation data { struct ekf2_innovations_s innovations = {}; innovations.timestamp = _replay_mode ? now : hrt_absolute_time(); _ekf.get_vel_pos_innov(&innovations.vel_pos_innov[0]); _ekf.get_mag_innov(&innovations.mag_innov[0]); _ekf.get_heading_innov(&innovations.heading_innov); _ekf.get_airspeed_innov(&innovations.airspeed_innov); _ekf.get_beta_innov(&innovations.beta_innov); _ekf.get_flow_innov(&innovations.flow_innov[0]); _ekf.get_hagl_innov(&innovations.hagl_innov); _ekf.get_vel_pos_innov_var(&innovations.vel_pos_innov_var[0]); _ekf.get_mag_innov_var(&innovations.mag_innov_var[0]); _ekf.get_heading_innov_var(&innovations.heading_innov_var); _ekf.get_airspeed_innov_var(&innovations.airspeed_innov_var); _ekf.get_beta_innov_var(&innovations.beta_innov_var); _ekf.get_flow_innov_var(&innovations.flow_innov_var[0]); _ekf.get_hagl_innov_var(&innovations.hagl_innov_var); _ekf.get_output_tracking_error(&innovations.output_tracking_error[0]); if (_estimator_innovations_pub == nullptr) { _estimator_innovations_pub = orb_advertise(ORB_ID(ekf2_innovations), &innovations); } else { orb_publish(ORB_ID(ekf2_innovations), _estimator_innovations_pub, &innovations); } } // save the declination to the EKF2_MAG_DECL parameter when a land event is detected if ((_params->mag_declination_source & (1 << 1)) && !_prev_landed && vehicle_land_detected.landed) { float decl_deg; _ekf.copy_mag_decl_deg(&decl_deg); _mag_declination_deg.set(decl_deg); } _prev_landed = vehicle_land_detected.landed; // publish ekf2_timestamps (using 0.1 ms relative timestamps) { ekf2_timestamps_s ekf2_timestamps; ekf2_timestamps.timestamp = sensors.timestamp; if (gps_updated) { // divide individually to get consistent rounding behavior ekf2_timestamps.gps_timestamp_rel = (int16_t)((int64_t)gps.timestamp / 100 - (int64_t)ekf2_timestamps.timestamp / 100); } else { ekf2_timestamps.gps_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID; } if (optical_flow_updated) { ekf2_timestamps.optical_flow_timestamp_rel = (int16_t)((int64_t)optical_flow.timestamp / 100 - (int64_t)ekf2_timestamps.timestamp / 100); } else { ekf2_timestamps.optical_flow_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID; } if (range_finder_updated) { ekf2_timestamps.distance_sensor_timestamp_rel = (int16_t)((int64_t)range_finder.timestamp / 100 - (int64_t)ekf2_timestamps.timestamp / 100); } else { ekf2_timestamps.distance_sensor_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID; } if (airspeed_updated) { ekf2_timestamps.airspeed_timestamp_rel = (int16_t)((int64_t)airspeed.timestamp / 100 - (int64_t)ekf2_timestamps.timestamp / 100); } else { ekf2_timestamps.airspeed_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID; } if (vision_position_updated) { ekf2_timestamps.vision_position_timestamp_rel = (int16_t)((int64_t)ev_pos.timestamp / 100 - (int64_t)ekf2_timestamps.timestamp / 100); } else { ekf2_timestamps.vision_position_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID; } if (vision_attitude_updated) { ekf2_timestamps.vision_attitude_timestamp_rel = (int16_t)((int64_t)ev_att.timestamp / 100 - (int64_t)ekf2_timestamps.timestamp / 100); } else { ekf2_timestamps.vision_attitude_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID; } if (_ekf2_timestamps_pub == nullptr) { _ekf2_timestamps_pub = orb_advertise(ORB_ID(ekf2_timestamps), &ekf2_timestamps); } else { orb_publish(ORB_ID(ekf2_timestamps), _ekf2_timestamps_pub, &ekf2_timestamps); } } // publish replay message if in replay mode bool publish_replay_message = (bool)_param_record_replay_msg.get(); if (publish_replay_message) { struct ekf2_replay_s replay = {}; replay.timestamp = now; replay.gyro_integral_dt = sensors.gyro_integral_dt; replay.accelerometer_integral_dt = sensors.accelerometer_integral_dt; replay.magnetometer_timestamp = _timestamp_mag_us; replay.baro_timestamp = _timestamp_balt_us; memcpy(replay.gyro_rad, sensors.gyro_rad, sizeof(replay.gyro_rad)); memcpy(replay.accelerometer_m_s2, sensors.accelerometer_m_s2, sizeof(replay.accelerometer_m_s2)); memcpy(replay.magnetometer_ga, sensors.magnetometer_ga, sizeof(replay.magnetometer_ga)); replay.baro_alt_meter = sensors.baro_alt_meter; // only write gps data if we had a gps update. if (gps_updated) { replay.time_usec = gps.timestamp; replay.lat = gps.lat; replay.lon = gps.lon; replay.alt = gps.alt; replay.fix_type = gps.fix_type; replay.nsats = gps.satellites_used; replay.eph = gps.eph; replay.epv = gps.epv; replay.sacc = gps.s_variance_m_s; replay.vel_m_s = gps.vel_m_s; replay.vel_n_m_s = gps.vel_n_m_s; replay.vel_e_m_s = gps.vel_e_m_s; replay.vel_d_m_s = gps.vel_d_m_s; replay.vel_ned_valid = gps.vel_ned_valid; } else { // this will tell the logging app not to bother logging any gps replay data replay.time_usec = 0; } if (optical_flow_updated) { replay.flow_timestamp = optical_flow.timestamp; replay.flow_pixel_integral[0] = optical_flow.pixel_flow_x_integral; replay.flow_pixel_integral[1] = optical_flow.pixel_flow_y_integral; replay.flow_gyro_integral[0] = optical_flow.gyro_x_rate_integral; replay.flow_gyro_integral[1] = optical_flow.gyro_y_rate_integral; replay.flow_time_integral = optical_flow.integration_timespan; replay.flow_quality = optical_flow.quality; } else { replay.flow_timestamp = 0; } if (range_finder_updated) { replay.rng_timestamp = range_finder.timestamp; replay.range_to_ground = range_finder.current_distance; } else { replay.rng_timestamp = 0; } if (airspeed_updated) { replay.asp_timestamp = airspeed.timestamp; replay.indicated_airspeed_m_s = airspeed.indicated_airspeed_m_s; replay.true_airspeed_m_s = airspeed.true_airspeed_m_s; } else { replay.asp_timestamp = 0; } if (vision_position_updated || vision_attitude_updated) { replay.ev_timestamp = vision_position_updated ? ev_pos.timestamp : ev_att.timestamp; replay.pos_ev[0] = ev_pos.x; replay.pos_ev[1] = ev_pos.y; replay.pos_ev[2] = ev_pos.z; replay.quat_ev[0] = ev_att.q[0]; replay.quat_ev[1] = ev_att.q[1]; replay.quat_ev[2] = ev_att.q[2]; replay.quat_ev[3] = ev_att.q[3]; // TODO : switch to covariances from topic later replay.pos_err = _default_ev_pos_noise; replay.ang_err = _default_ev_ang_noise; } else { replay.ev_timestamp = 0; } if (_replay_pub == nullptr) { _replay_pub = orb_advertise(ORB_ID(ekf2_replay), &replay); } else { orb_publish(ORB_ID(ekf2_replay), _replay_pub, &replay); } } } orb_unsubscribe(sensors_sub); orb_unsubscribe(gps_sub); orb_unsubscribe(airspeed_sub); orb_unsubscribe(params_sub); orb_unsubscribe(optical_flow_sub); orb_unsubscribe(range_finder_sub); orb_unsubscribe(ev_pos_sub); orb_unsubscribe(vehicle_land_detected_sub); orb_unsubscribe(status_sub); delete ekf2::instance; ekf2::instance = nullptr; }
/**************************************************************************** * main ****************************************************************************/ int position_estimator_inav_thread_main(int argc, char *argv[]) { int mavlink_fd; mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0); float x_est[2] = { 0.0f, 0.0f }; // pos, vel float y_est[2] = { 0.0f, 0.0f }; // pos, vel float z_est[2] = { 0.0f, 0.0f }; // pos, vel float est_buf[EST_BUF_SIZE][3][2]; // estimated position buffer float R_buf[EST_BUF_SIZE][3][3]; // rotation matrix buffer float R_gps[3][3]; // rotation matrix for GPS correction moment memset(est_buf, 0, sizeof(est_buf)); memset(R_buf, 0, sizeof(R_buf)); memset(R_gps, 0, sizeof(R_gps)); int buf_ptr = 0; static const float min_eph_epv = 2.0f; // min EPH/EPV, used for weight calculation static const float max_eph_epv = 20.0f; // max EPH/EPV acceptable for estimation float eph = max_eph_epv; float epv = 1.0f; float eph_flow = 1.0f; float eph_vision = 0.2f; float epv_vision = 0.2f; float eph_mocap = 0.05f; float epv_mocap = 0.05f; float x_est_prev[2], y_est_prev[2], z_est_prev[2]; memset(x_est_prev, 0, sizeof(x_est_prev)); memset(y_est_prev, 0, sizeof(y_est_prev)); memset(z_est_prev, 0, sizeof(z_est_prev)); int baro_init_cnt = 0; int baro_init_num = 200; float baro_offset = 0.0f; // baro offset for reference altitude, initialized on start, then adjusted float surface_offset = 0.0f; // ground level offset from reference altitude float surface_offset_rate = 0.0f; // surface offset change rate hrt_abstime accel_timestamp = 0; hrt_abstime baro_timestamp = 0; bool ref_inited = false; hrt_abstime ref_init_start = 0; const hrt_abstime ref_init_delay = 1000000; // wait for 1s after 3D fix struct map_projection_reference_s ref; memset(&ref, 0, sizeof(ref)); hrt_abstime home_timestamp = 0; uint16_t accel_updates = 0; uint16_t baro_updates = 0; uint16_t gps_updates = 0; uint16_t attitude_updates = 0; uint16_t flow_updates = 0; uint16_t vision_updates = 0; uint16_t mocap_updates = 0; hrt_abstime updates_counter_start = hrt_absolute_time(); hrt_abstime pub_last = hrt_absolute_time(); hrt_abstime t_prev = 0; /* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */ float acc[] = { 0.0f, 0.0f, 0.0f }; // N E D float acc_bias[] = { 0.0f, 0.0f, 0.0f }; // body frame float corr_baro = 0.0f; // D float corr_gps[3][2] = { { 0.0f, 0.0f }, // N (pos, vel) { 0.0f, 0.0f }, // E (pos, vel) { 0.0f, 0.0f }, // D (pos, vel) }; float w_gps_xy = 1.0f; float w_gps_z = 1.0f; float corr_vision[3][2] = { { 0.0f, 0.0f }, // N (pos, vel) { 0.0f, 0.0f }, // E (pos, vel) { 0.0f, 0.0f }, // D (pos, vel) }; float corr_mocap[3][1] = { { 0.0f }, // N (pos) { 0.0f }, // E (pos) { 0.0f }, // D (pos) }; float corr_sonar = 0.0f; float corr_sonar_filtered = 0.0f; float corr_flow[] = { 0.0f, 0.0f }; // N E float w_flow = 0.0f; float sonar_prev = 0.0f; //hrt_abstime flow_prev = 0; // time of last flow measurement hrt_abstime sonar_time = 0; // time of last sonar measurement (not filtered) hrt_abstime sonar_valid_time = 0; // time of last sonar measurement used for correction (filtered) bool gps_valid = false; // GPS is valid bool sonar_valid = false; // sonar is valid bool flow_valid = false; // flow is valid bool flow_accurate = false; // flow should be accurate (this flag not updated if flow_valid == false) bool vision_valid = false; // vision is valid bool mocap_valid = false; // mocap is valid /* declare and safely initialize all structs */ struct actuator_controls_s actuator; memset(&actuator, 0, sizeof(actuator)); struct actuator_armed_s armed; memset(&armed, 0, sizeof(armed)); struct sensor_combined_s sensor; memset(&sensor, 0, sizeof(sensor)); struct vehicle_gps_position_s gps; memset(&gps, 0, sizeof(gps)); struct home_position_s home; memset(&home, 0, sizeof(home)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); struct vehicle_local_position_s local_pos; memset(&local_pos, 0, sizeof(local_pos)); struct optical_flow_s flow; memset(&flow, 0, sizeof(flow)); struct vision_position_estimate_s vision; memset(&vision, 0, sizeof(vision)); struct att_pos_mocap_s mocap; memset(&mocap, 0, sizeof(mocap)); struct vehicle_global_position_s global_pos; memset(&global_pos, 0, sizeof(global_pos)); /* subscribe */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); int actuator_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow)); int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); int vision_position_estimate_sub = orb_subscribe(ORB_ID(vision_position_estimate)); int att_pos_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap)); int home_position_sub = orb_subscribe(ORB_ID(home_position)); /* advertise */ orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos); orb_advert_t vehicle_global_position_pub = NULL; struct position_estimator_inav_params params; struct position_estimator_inav_param_handles pos_inav_param_handles; /* initialize parameter handles */ inav_parameters_init(&pos_inav_param_handles); /* first parameters read at start up */ struct parameter_update_s param_update; orb_copy(ORB_ID(parameter_update), parameter_update_sub, ¶m_update); /* read from param topic to clear updated flag */ /* first parameters update */ inav_parameters_update(&pos_inav_param_handles, ¶ms); px4_pollfd_struct_t fds_init[1] = { { .fd = sensor_combined_sub, .events = POLLIN }, }; /* wait for initial baro value */ bool wait_baro = true; thread_running = true; while (wait_baro && !thread_should_exit) { int ret = px4_poll(fds_init, 1, 1000); if (ret < 0) { /* poll error */ mavlink_log_info(mavlink_fd, "[inav] poll error on init"); } else if (ret > 0) { if (fds_init[0].revents & POLLIN) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); if (wait_baro && sensor.baro_timestamp != baro_timestamp) { baro_timestamp = sensor.baro_timestamp; /* mean calculation over several measurements */ if (baro_init_cnt < baro_init_num) { if (PX4_ISFINITE(sensor.baro_alt_meter)) { baro_offset += sensor.baro_alt_meter; baro_init_cnt++; } } else { wait_baro = false; baro_offset /= (float) baro_init_cnt; warnx("baro offset: %d m", (int)baro_offset); mavlink_log_info(mavlink_fd, "[inav] baro offset: %d m", (int)baro_offset); local_pos.z_valid = true; local_pos.v_z_valid = true; } } } } } /* main loop */ px4_pollfd_struct_t fds[1] = { { .fd = vehicle_attitude_sub, .events = POLLIN }, }; while (!thread_should_exit) { int ret = px4_poll(fds, 1, 20); // wait maximal 20 ms = 50 Hz minimum rate hrt_abstime t = hrt_absolute_time(); if (ret < 0) { /* poll error */ mavlink_log_info(mavlink_fd, "[inav] poll error on init"); continue; } else if (ret > 0) { /* act on attitude updates */ /* vehicle attitude */ orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); attitude_updates++; bool updated; /* parameter update */ orb_check(parameter_update_sub, &updated); if (updated) { struct parameter_update_s update; orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); inav_parameters_update(&pos_inav_param_handles, ¶ms); } /* actuator */ orb_check(actuator_sub, &updated); if (updated) { orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_sub, &actuator); } /* armed */ orb_check(armed_sub, &updated); if (updated) { orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); } /* sensor combined */ orb_check(sensor_combined_sub, &updated); if (updated) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); if (sensor.accelerometer_timestamp != accel_timestamp) { if (att.R_valid) { /* correct accel bias */ sensor.accelerometer_m_s2[0] -= acc_bias[0]; sensor.accelerometer_m_s2[1] -= acc_bias[1]; sensor.accelerometer_m_s2[2] -= acc_bias[2]; /* transform acceleration vector from body frame to NED frame */ for (int i = 0; i < 3; i++) { acc[i] = 0.0f; for (int j = 0; j < 3; j++) { acc[i] += PX4_R(att.R, i, j) * sensor.accelerometer_m_s2[j]; } } acc[2] += CONSTANTS_ONE_G; } else { memset(acc, 0, sizeof(acc)); } accel_timestamp = sensor.accelerometer_timestamp; accel_updates++; } if (sensor.baro_timestamp != baro_timestamp) { corr_baro = baro_offset - sensor.baro_alt_meter - z_est[0]; baro_timestamp = sensor.baro_timestamp; baro_updates++; } } /* optical flow */ orb_check(optical_flow_sub, &updated); if (updated) { orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); /* calculate time from previous update */ // float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f; // flow_prev = flow.flow_timestamp; if ((flow.ground_distance_m > 0.31f) && (flow.ground_distance_m < 4.0f) && (PX4_R(att.R, 2, 2) > 0.7f) && (fabsf(flow.ground_distance_m - sonar_prev) > FLT_EPSILON)) { sonar_time = t; sonar_prev = flow.ground_distance_m; corr_sonar = flow.ground_distance_m + surface_offset + z_est[0]; corr_sonar_filtered += (corr_sonar - corr_sonar_filtered) * params.sonar_filt; if (fabsf(corr_sonar) > params.sonar_err) { /* correction is too large: spike or new ground level? */ if (fabsf(corr_sonar - corr_sonar_filtered) > params.sonar_err) { /* spike detected, ignore */ corr_sonar = 0.0f; sonar_valid = false; } else { /* new ground level */ surface_offset -= corr_sonar; surface_offset_rate = 0.0f; corr_sonar = 0.0f; corr_sonar_filtered = 0.0f; sonar_valid_time = t; sonar_valid = true; local_pos.surface_bottom_timestamp = t; mavlink_log_info(mavlink_fd, "[inav] new surface level: %d", (int)surface_offset); } } else { /* correction is ok, use it */ sonar_valid_time = t; sonar_valid = true; } } float flow_q = flow.quality / 255.0f; float dist_bottom = - z_est[0] - surface_offset; if (dist_bottom > 0.3f && flow_q > params.flow_q_min && (t < sonar_valid_time + sonar_valid_timeout) && PX4_R(att.R, 2, 2) > 0.7f) { /* distance to surface */ float flow_dist = dist_bottom / PX4_R(att.R, 2, 2); /* check if flow if too large for accurate measurements */ /* calculate estimated velocity in body frame */ float body_v_est[2] = { 0.0f, 0.0f }; for (int i = 0; i < 2; i++) { body_v_est[i] = PX4_R(att.R, 0, i) * x_est[1] + PX4_R(att.R, 1, i) * y_est[1] + PX4_R(att.R, 2, i) * z_est[1]; } /* set this flag if flow should be accurate according to current velocity and attitude rate estimate */ flow_accurate = fabsf(body_v_est[1] / flow_dist - att.rollspeed) < max_flow && fabsf(body_v_est[0] / flow_dist + att.pitchspeed) < max_flow; /* convert raw flow to angular flow (rad/s) */ float flow_ang[2]; //todo check direction of x und y axis flow_ang[0] = flow.pixel_flow_x_integral/(float)flow.integration_timespan*1000000.0f;//flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt; flow_ang[1] = flow.pixel_flow_y_integral/(float)flow.integration_timespan*1000000.0f;//flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt; /* flow measurements vector */ float flow_m[3]; flow_m[0] = -flow_ang[0] * flow_dist; flow_m[1] = -flow_ang[1] * flow_dist; flow_m[2] = z_est[1]; /* velocity in NED */ float flow_v[2] = { 0.0f, 0.0f }; /* project measurements vector to NED basis, skip Z component */ for (int i = 0; i < 2; i++) { for (int j = 0; j < 3; j++) { flow_v[i] += PX4_R(att.R, i, j) * flow_m[j]; } } /* velocity correction */ corr_flow[0] = flow_v[0] - x_est[1]; corr_flow[1] = flow_v[1] - y_est[1]; /* adjust correction weight */ float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min); w_flow = PX4_R(att.R, 2, 2) * flow_q_weight / fmaxf(1.0f, flow_dist); /* if flow is not accurate, reduce weight for it */ // TODO make this more fuzzy if (!flow_accurate) { w_flow *= 0.05f; } /* under ideal conditions, on 1m distance assume EPH = 10cm */ eph_flow = 0.1f / w_flow; flow_valid = true; } else { w_flow = 0.0f; flow_valid = false; } flow_updates++; } /* home position */ orb_check(home_position_sub, &updated); if (updated) { orb_copy(ORB_ID(home_position), home_position_sub, &home); if (home.timestamp != home_timestamp) { home_timestamp = home.timestamp; double est_lat, est_lon; float est_alt; if (ref_inited) { /* calculate current estimated position in global frame */ est_alt = local_pos.ref_alt - local_pos.z; map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon); } /* update reference */ map_projection_init(&ref, home.lat, home.lon); /* update baro offset */ baro_offset += home.alt - local_pos.ref_alt; local_pos.ref_lat = home.lat; local_pos.ref_lon = home.lon; local_pos.ref_alt = home.alt; local_pos.ref_timestamp = home.timestamp; if (ref_inited) { /* reproject position estimate with new reference */ map_projection_project(&ref, est_lat, est_lon, &x_est[0], &y_est[0]); z_est[0] = -(est_alt - local_pos.ref_alt); } ref_inited = true; } } /* check no vision circuit breaker is set */ if (params.no_vision != CBRK_NO_VISION_KEY) { /* vehicle vision position */ orb_check(vision_position_estimate_sub, &updated); if (updated) { orb_copy(ORB_ID(vision_position_estimate), vision_position_estimate_sub, &vision); static float last_vision_x = 0.0f; static float last_vision_y = 0.0f; static float last_vision_z = 0.0f; /* reset position estimate on first vision update */ if (!vision_valid) { x_est[0] = vision.x; x_est[1] = vision.vx; y_est[0] = vision.y; y_est[1] = vision.vy; /* only reset the z estimate if the z weight parameter is not zero */ if (params.w_z_vision_p > MIN_VALID_W) { z_est[0] = vision.z; z_est[1] = vision.vz; } vision_valid = true; last_vision_x = vision.x; last_vision_y = vision.y; last_vision_z = vision.z; warnx("VISION estimate valid"); mavlink_log_info(mavlink_fd, "[inav] VISION estimate valid"); } /* calculate correction for position */ corr_vision[0][0] = vision.x - x_est[0]; corr_vision[1][0] = vision.y - y_est[0]; corr_vision[2][0] = vision.z - z_est[0]; static hrt_abstime last_vision_time = 0; float vision_dt = (vision.timestamp_boot - last_vision_time) / 1e6f; last_vision_time = vision.timestamp_boot; if (vision_dt > 0.000001f && vision_dt < 0.2f) { vision.vx = (vision.x - last_vision_x) / vision_dt; vision.vy = (vision.y - last_vision_y) / vision_dt; vision.vz = (vision.z - last_vision_z) / vision_dt; last_vision_x = vision.x; last_vision_y = vision.y; last_vision_z = vision.z; /* calculate correction for velocity */ corr_vision[0][1] = vision.vx - x_est[1]; corr_vision[1][1] = vision.vy - y_est[1]; corr_vision[2][1] = vision.vz - z_est[1]; } else { /* assume zero motion */ corr_vision[0][1] = 0.0f - x_est[1]; corr_vision[1][1] = 0.0f - y_est[1]; corr_vision[2][1] = 0.0f - z_est[1]; } vision_updates++; } } /* vehicle mocap position */ orb_check(att_pos_mocap_sub, &updated); if (updated) { orb_copy(ORB_ID(att_pos_mocap), att_pos_mocap_sub, &mocap); /* reset position estimate on first mocap update */ if (!mocap_valid) { x_est[0] = mocap.x; y_est[0] = mocap.y; z_est[0] = mocap.z; mocap_valid = true; warnx("MOCAP data valid"); mavlink_log_info(mavlink_fd, "[inav] MOCAP data valid"); } /* calculate correction for position */ corr_mocap[0][0] = mocap.x - x_est[0]; corr_mocap[1][0] = mocap.y - y_est[0]; corr_mocap[2][0] = mocap.z - z_est[0]; mocap_updates++; } /* vehicle GPS position */ orb_check(vehicle_gps_position_sub, &updated); if (updated) { orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); bool reset_est = false; /* hysteresis for GPS quality */ if (gps_valid) { if (gps.eph > max_eph_epv || gps.epv > max_eph_epv || gps.fix_type < 3) { gps_valid = false; mavlink_log_info(mavlink_fd, "[inav] GPS signal lost"); } } else { if (gps.eph < max_eph_epv * 0.7f && gps.epv < max_eph_epv * 0.7f && gps.fix_type >= 3) { gps_valid = true; reset_est = true; mavlink_log_info(mavlink_fd, "[inav] GPS signal found"); } } if (gps_valid) { double lat = gps.lat * 1e-7; double lon = gps.lon * 1e-7; float alt = gps.alt * 1e-3; /* initialize reference position if needed */ if (!ref_inited) { if (ref_init_start == 0) { ref_init_start = t; } else if (t > ref_init_start + ref_init_delay) { ref_inited = true; /* set position estimate to (0, 0, 0), use GPS velocity for XY */ x_est[0] = 0.0f; x_est[1] = gps.vel_n_m_s; y_est[0] = 0.0f; y_est[1] = gps.vel_e_m_s; local_pos.ref_lat = lat; local_pos.ref_lon = lon; local_pos.ref_alt = alt + z_est[0]; local_pos.ref_timestamp = t; /* initialize projection */ map_projection_init(&ref, lat, lon); // XXX replace this print warnx("init ref: lat=%.7f, lon=%.7f, alt=%8.4f", (double)lat, (double)lon, (double)alt); mavlink_log_info(mavlink_fd, "[inav] init ref: %.7f, %.7f, %8.4f", (double)lat, (double)lon, (double)alt); } } if (ref_inited) { /* project GPS lat lon to plane */ float gps_proj[2]; map_projection_project(&ref, lat, lon, &(gps_proj[0]), &(gps_proj[1])); /* reset position estimate when GPS becomes good */ if (reset_est) { x_est[0] = gps_proj[0]; x_est[1] = gps.vel_n_m_s; y_est[0] = gps_proj[1]; y_est[1] = gps.vel_e_m_s; } /* calculate index of estimated values in buffer */ int est_i = buf_ptr - 1 - min(EST_BUF_SIZE - 1, max(0, (int)(params.delay_gps * 1000000.0f / PUB_INTERVAL))); if (est_i < 0) { est_i += EST_BUF_SIZE; } /* calculate correction for position */ corr_gps[0][0] = gps_proj[0] - est_buf[est_i][0][0]; corr_gps[1][0] = gps_proj[1] - est_buf[est_i][1][0]; corr_gps[2][0] = local_pos.ref_alt - alt - est_buf[est_i][2][0]; /* calculate correction for velocity */ if (gps.vel_ned_valid) { corr_gps[0][1] = gps.vel_n_m_s - est_buf[est_i][0][1]; corr_gps[1][1] = gps.vel_e_m_s - est_buf[est_i][1][1]; corr_gps[2][1] = gps.vel_d_m_s - est_buf[est_i][2][1]; } else { corr_gps[0][1] = 0.0f; corr_gps[1][1] = 0.0f; corr_gps[2][1] = 0.0f; } /* save rotation matrix at this moment */ memcpy(R_gps, R_buf[est_i], sizeof(R_gps)); w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph); w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv); } } else { /* no GPS lock */ memset(corr_gps, 0, sizeof(corr_gps)); ref_init_start = 0; } gps_updates++; } } /* check for timeout on FLOW topic */ if ((flow_valid || sonar_valid) && t > flow.timestamp + flow_topic_timeout) { flow_valid = false; sonar_valid = false; warnx("FLOW timeout"); mavlink_log_info(mavlink_fd, "[inav] FLOW timeout"); } /* check for timeout on GPS topic */ if (gps_valid && (t > (gps.timestamp_position + gps_topic_timeout))) { gps_valid = false; warnx("GPS timeout"); mavlink_log_info(mavlink_fd, "[inav] GPS timeout"); } /* check for timeout on vision topic */ if (vision_valid && (t > (vision.timestamp_boot + vision_topic_timeout))) { vision_valid = false; warnx("VISION timeout"); mavlink_log_info(mavlink_fd, "[inav] VISION timeout"); } /* check for timeout on mocap topic */ if (mocap_valid && (t > (mocap.timestamp_boot + mocap_topic_timeout))) { mocap_valid = false; warnx("MOCAP timeout"); mavlink_log_info(mavlink_fd, "[inav] MOCAP timeout"); } /* check for sonar measurement timeout */ if (sonar_valid && (t > (sonar_time + sonar_timeout))) { corr_sonar = 0.0f; sonar_valid = false; } float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f; dt = fmaxf(fminf(0.02, dt), 0.002); // constrain dt from 2 to 20 ms t_prev = t; /* increase EPH/EPV on each step */ if (eph < max_eph_epv) { eph *= 1.0f + dt; } if (epv < max_eph_epv) { epv += 0.005f * dt; // add 1m to EPV each 200s (baro drift) } /* use GPS if it's valid and reference position initialized */ bool use_gps_xy = ref_inited && gps_valid && params.w_xy_gps_p > MIN_VALID_W; bool use_gps_z = ref_inited && gps_valid && params.w_z_gps_p > MIN_VALID_W; /* use VISION if it's valid and has a valid weight parameter */ bool use_vision_xy = vision_valid && params.w_xy_vision_p > MIN_VALID_W; bool use_vision_z = vision_valid && params.w_z_vision_p > MIN_VALID_W; /* use MOCAP if it's valid and has a valid weight parameter */ bool use_mocap = mocap_valid && params.w_mocap_p > MIN_VALID_W; /* use flow if it's valid and (accurate or no GPS available) */ bool use_flow = flow_valid && (flow_accurate || !use_gps_xy); bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || use_vision_xy || use_mocap; bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout); if (dist_bottom_valid) { /* surface distance prediction */ surface_offset += surface_offset_rate * dt; /* surface distance correction */ if (sonar_valid) { surface_offset_rate -= corr_sonar * 0.5f * params.w_z_sonar * params.w_z_sonar * dt; surface_offset -= corr_sonar * params.w_z_sonar * dt; } } float w_xy_gps_p = params.w_xy_gps_p * w_gps_xy; float w_xy_gps_v = params.w_xy_gps_v * w_gps_xy; float w_z_gps_p = params.w_z_gps_p * w_gps_z; float w_z_gps_v = params.w_z_gps_v * w_gps_z; float w_xy_vision_p = params.w_xy_vision_p; float w_xy_vision_v = params.w_xy_vision_v; float w_z_vision_p = params.w_z_vision_p; float w_mocap_p = params.w_mocap_p; /* reduce GPS weight if optical flow is good */ if (use_flow && flow_accurate) { w_xy_gps_p *= params.w_gps_flow; w_xy_gps_v *= params.w_gps_flow; } /* baro offset correction */ if (use_gps_z) { float offs_corr = corr_gps[2][0] * w_z_gps_p * dt; baro_offset += offs_corr; corr_baro += offs_corr; } /* accelerometer bias correction for GPS (use buffered rotation matrix) */ float accel_bias_corr[3] = { 0.0f, 0.0f, 0.0f }; if (use_gps_xy) { accel_bias_corr[0] -= corr_gps[0][0] * w_xy_gps_p * w_xy_gps_p; accel_bias_corr[0] -= corr_gps[0][1] * w_xy_gps_v; accel_bias_corr[1] -= corr_gps[1][0] * w_xy_gps_p * w_xy_gps_p; accel_bias_corr[1] -= corr_gps[1][1] * w_xy_gps_v; } if (use_gps_z) { accel_bias_corr[2] -= corr_gps[2][0] * w_z_gps_p * w_z_gps_p; accel_bias_corr[2] -= corr_gps[2][1] * w_z_gps_v; } /* transform error vector from NED frame to body frame */ for (int i = 0; i < 3; i++) { float c = 0.0f; for (int j = 0; j < 3; j++) { c += R_gps[j][i] * accel_bias_corr[j]; } if (isfinite(c)) { acc_bias[i] += c * params.w_acc_bias * dt; } } /* accelerometer bias correction for VISION (use buffered rotation matrix) */ accel_bias_corr[0] = 0.0f; accel_bias_corr[1] = 0.0f; accel_bias_corr[2] = 0.0f; if (use_vision_xy) { accel_bias_corr[0] -= corr_vision[0][0] * w_xy_vision_p * w_xy_vision_p; accel_bias_corr[0] -= corr_vision[0][1] * w_xy_vision_v; accel_bias_corr[1] -= corr_vision[1][0] * w_xy_vision_p * w_xy_vision_p; accel_bias_corr[1] -= corr_vision[1][1] * w_xy_vision_v; } if (use_vision_z) { accel_bias_corr[2] -= corr_vision[2][0] * w_z_vision_p * w_z_vision_p; } /* accelerometer bias correction for MOCAP (use buffered rotation matrix) */ accel_bias_corr[0] = 0.0f; accel_bias_corr[1] = 0.0f; accel_bias_corr[2] = 0.0f; if (use_mocap) { accel_bias_corr[0] -= corr_mocap[0][0] * w_mocap_p * w_mocap_p; accel_bias_corr[1] -= corr_mocap[1][0] * w_mocap_p * w_mocap_p; accel_bias_corr[2] -= corr_mocap[2][0] * w_mocap_p * w_mocap_p; } /* transform error vector from NED frame to body frame */ for (int i = 0; i < 3; i++) { float c = 0.0f; for (int j = 0; j < 3; j++) { c += PX4_R(att.R, j, i) * accel_bias_corr[j]; } if (isfinite(c)) { acc_bias[i] += c * params.w_acc_bias * dt; } } /* accelerometer bias correction for flow and baro (assume that there is no delay) */ accel_bias_corr[0] = 0.0f; accel_bias_corr[1] = 0.0f; accel_bias_corr[2] = 0.0f; if (use_flow) { accel_bias_corr[0] -= corr_flow[0] * params.w_xy_flow; accel_bias_corr[1] -= corr_flow[1] * params.w_xy_flow; } accel_bias_corr[2] -= corr_baro * params.w_z_baro * params.w_z_baro; /* transform error vector from NED frame to body frame */ for (int i = 0; i < 3; i++) { float c = 0.0f; for (int j = 0; j < 3; j++) { c += PX4_R(att.R, j, i) * accel_bias_corr[j]; } if (isfinite(c)) { acc_bias[i] += c * params.w_acc_bias * dt; } } /* inertial filter prediction for altitude */ inertial_filter_predict(dt, z_est, acc[2]); if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) { write_debug_log("BAD ESTIMATE AFTER Z PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); memcpy(z_est, z_est_prev, sizeof(z_est)); } /* inertial filter correction for altitude */ inertial_filter_correct(corr_baro, dt, z_est, 0, params.w_z_baro); if (use_gps_z) { epv = fminf(epv, gps.epv); inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p); inertial_filter_correct(corr_gps[2][1], dt, z_est, 1, w_z_gps_v); } if (use_vision_z) { epv = fminf(epv, epv_vision); inertial_filter_correct(corr_vision[2][0], dt, z_est, 0, w_z_vision_p); } if (use_mocap) { epv = fminf(epv, epv_mocap); inertial_filter_correct(corr_mocap[2][0], dt, z_est, 0, w_mocap_p); } if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) { write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); memcpy(z_est, z_est_prev, sizeof(z_est)); memset(corr_gps, 0, sizeof(corr_gps)); memset(corr_vision, 0, sizeof(corr_vision)); memset(corr_mocap, 0, sizeof(corr_mocap)); corr_baro = 0; } else { memcpy(z_est_prev, z_est, sizeof(z_est)); } if (can_estimate_xy) { /* inertial filter prediction for position */ inertial_filter_predict(dt, x_est, acc[0]); inertial_filter_predict(dt, y_est, acc[1]); if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && isfinite(y_est[1]))) { write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); memcpy(x_est, x_est_prev, sizeof(x_est)); memcpy(y_est, y_est_prev, sizeof(y_est)); } /* inertial filter correction for position */ if (use_flow) { eph = fminf(eph, eph_flow); inertial_filter_correct(corr_flow[0], dt, x_est, 1, params.w_xy_flow * w_flow); inertial_filter_correct(corr_flow[1], dt, y_est, 1, params.w_xy_flow * w_flow); } if (use_gps_xy) { eph = fminf(eph, gps.eph); inertial_filter_correct(corr_gps[0][0], dt, x_est, 0, w_xy_gps_p); inertial_filter_correct(corr_gps[1][0], dt, y_est, 0, w_xy_gps_p); if (gps.vel_ned_valid && t < gps.timestamp_velocity + gps_topic_timeout) { inertial_filter_correct(corr_gps[0][1], dt, x_est, 1, w_xy_gps_v); inertial_filter_correct(corr_gps[1][1], dt, y_est, 1, w_xy_gps_v); } } if (use_vision_xy) { eph = fminf(eph, eph_vision); inertial_filter_correct(corr_vision[0][0], dt, x_est, 0, w_xy_vision_p); inertial_filter_correct(corr_vision[1][0], dt, y_est, 0, w_xy_vision_p); if (w_xy_vision_v > MIN_VALID_W) { inertial_filter_correct(corr_vision[0][1], dt, x_est, 1, w_xy_vision_v); inertial_filter_correct(corr_vision[1][1], dt, y_est, 1, w_xy_vision_v); } } if (use_mocap) { eph = fminf(eph, eph_mocap); inertial_filter_correct(corr_mocap[0][0], dt, x_est, 0, w_mocap_p); inertial_filter_correct(corr_mocap[1][0], dt, y_est, 0, w_mocap_p); } if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && isfinite(y_est[1]))) { write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); memcpy(x_est, x_est_prev, sizeof(x_est)); memcpy(y_est, y_est_prev, sizeof(y_est)); memset(corr_gps, 0, sizeof(corr_gps)); memset(corr_vision, 0, sizeof(corr_vision)); memset(corr_mocap, 0, sizeof(corr_mocap)); memset(corr_flow, 0, sizeof(corr_flow)); } else { memcpy(x_est_prev, x_est, sizeof(x_est)); memcpy(y_est_prev, y_est, sizeof(y_est)); } } else { /* gradually reset xy velocity estimates */ inertial_filter_correct(-x_est[1], dt, x_est, 1, params.w_xy_res_v); inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_res_v); } if (inav_verbose_mode) { /* print updates rate */ if (t > updates_counter_start + updates_counter_len) { float updates_dt = (t - updates_counter_start) * 0.000001f; warnx( "updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s, flow = %.1f/s, vision = %.1f/s, mocap = %.1f/s", (double)(accel_updates / updates_dt), (double)(baro_updates / updates_dt), (double)(gps_updates / updates_dt), (double)(attitude_updates / updates_dt), (double)(flow_updates / updates_dt), (double)(vision_updates / updates_dt), (double)(mocap_updates / updates_dt)); updates_counter_start = t; accel_updates = 0; baro_updates = 0; gps_updates = 0; attitude_updates = 0; flow_updates = 0; vision_updates = 0; mocap_updates = 0; } } if (t > pub_last + PUB_INTERVAL) { pub_last = t; /* push current estimate to buffer */ est_buf[buf_ptr][0][0] = x_est[0]; est_buf[buf_ptr][0][1] = x_est[1]; est_buf[buf_ptr][1][0] = y_est[0]; est_buf[buf_ptr][1][1] = y_est[1]; est_buf[buf_ptr][2][0] = z_est[0]; est_buf[buf_ptr][2][1] = z_est[1]; /* push current rotation matrix to buffer */ memcpy(R_buf[buf_ptr], att.R, sizeof(att.R)); buf_ptr++; if (buf_ptr >= EST_BUF_SIZE) { buf_ptr = 0; } /* publish local position */ local_pos.xy_valid = can_estimate_xy; local_pos.v_xy_valid = can_estimate_xy; local_pos.xy_global = local_pos.xy_valid && use_gps_xy; local_pos.z_global = local_pos.z_valid && use_gps_z; local_pos.x = x_est[0]; local_pos.vx = x_est[1]; local_pos.y = y_est[0]; local_pos.vy = y_est[1]; local_pos.z = z_est[0]; local_pos.vz = z_est[1]; local_pos.yaw = att.yaw; local_pos.dist_bottom_valid = dist_bottom_valid; local_pos.eph = eph; local_pos.epv = epv; if (local_pos.dist_bottom_valid) { local_pos.dist_bottom = -z_est[0] - surface_offset; local_pos.dist_bottom_rate = -z_est[1] - surface_offset_rate; } local_pos.timestamp = t; orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos); if (local_pos.xy_global && local_pos.z_global) { /* publish global position */ global_pos.timestamp = t; global_pos.time_utc_usec = gps.time_utc_usec; double est_lat, est_lon; map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon); global_pos.lat = est_lat; global_pos.lon = est_lon; global_pos.alt = local_pos.ref_alt - local_pos.z; global_pos.vel_n = local_pos.vx; global_pos.vel_e = local_pos.vy; global_pos.vel_d = local_pos.vz; global_pos.yaw = local_pos.yaw; global_pos.eph = eph; global_pos.epv = epv; if (vehicle_global_position_pub == NULL) { vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); } else { orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos); } } } } warnx("stopped"); mavlink_log_info(mavlink_fd, "[inav] stopped"); thread_running = false; return 0; }
/* * EKF Attitude Estimator main function. * * Estimates the attitude recursively once started. * * @param argc number of commandline arguments (plus command name) * @param argv strings containing the arguments */ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) { float dt = 0.005f; /* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */ float z_k[9] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 9.81f, 0.2f, -0.2f, 0.2f}; /**< Measurement vector */ float x_aposteriori_k[12]; /**< states */ float P_aposteriori_k[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 100.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f, }; /**< init: diagonal matrix with big values */ float x_aposteriori[12]; float P_aposteriori[144]; /* output euler angles */ float euler[3] = {0.0f, 0.0f, 0.0f}; float Rot_matrix[9] = {1.f, 0, 0, 0, 1.f, 0, 0, 0, 1.f }; /**< init: identity matrix */ float debugOutput[4] = { 0.0f }; /* Initialize filter */ AttitudeEKF_initialize(); struct sensor_combined_s raw; memset(&raw, 0, sizeof(raw)); struct vehicle_gps_position_s gps; memset(&gps, 0, sizeof(gps)); gps.eph = 100000; gps.epv = 100000; struct vehicle_global_position_s global_pos; memset(&global_pos, 0, sizeof(global_pos)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); struct vehicle_control_mode_s control_mode; memset(&control_mode, 0, sizeof(control_mode)); uint64_t last_data = 0; uint64_t last_measurement = 0; uint64_t last_vel_t = 0; /* current velocity */ math::Vector<3> vel; vel.zero(); /* previous velocity */ math::Vector<3> vel_prev; vel_prev.zero(); /* actual acceleration (by GPS velocity) in body frame */ math::Vector<3> acc; acc.zero(); /* rotation matrix */ math::Matrix<3, 3> R; R.identity(); /* subscribe to raw data */ int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); /* subscribe to GPS */ int sub_gps = orb_subscribe(ORB_ID(vehicle_gps_position)); /* subscribe to GPS */ int sub_global_pos = orb_subscribe(ORB_ID(vehicle_global_position)); /* subscribe to param changes */ int sub_params = orb_subscribe(ORB_ID(parameter_update)); /* subscribe to control mode */ int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode)); /* subscribe to vision estimate */ int vision_sub = orb_subscribe(ORB_ID(vision_position_estimate)); /* subscribe to mocap data */ int mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap)); /* advertise attitude */ orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); int loopcounter = 0; thread_running = true; /* keep track of sensor updates */ uint64_t sensor_last_timestamp[3] = {0, 0, 0}; struct attitude_estimator_ekf_params ekf_params; memset(&ekf_params, 0, sizeof(ekf_params)); struct attitude_estimator_ekf_param_handles ekf_param_handles = { 0 }; /* initialize parameter handles */ parameters_init(&ekf_param_handles); bool initialized = false; float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f }; /* magnetic declination, in radians */ float mag_decl = 0.0f; /* rotation matrix for magnetic declination */ math::Matrix<3, 3> R_decl; R_decl.identity(); struct vision_position_estimate_s vision {}; struct att_pos_mocap_s mocap {}; /* register the perf counter */ perf_counter_t ekf_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_ekf"); /* Main loop*/ while (!thread_should_exit) { px4_pollfd_struct_t fds[2]; fds[0].fd = sub_raw; fds[0].events = POLLIN; fds[1].fd = sub_params; fds[1].events = POLLIN; int ret = px4_poll(fds, 2, 1000); if (ret < 0) { /* XXX this is seriously bad - should be an emergency */ } else if (ret == 0) { /* check if we're in HIL - not getting sensor data is fine then */ orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode); if (!control_mode.flag_system_hil_enabled) { warnx("WARNING: Not getting sensor data - sensor app running?"); } } else { /* only update parameters if they changed */ if (fds[1].revents & POLLIN) { /* read from param to clear updated flag */ struct parameter_update_s update; orb_copy(ORB_ID(parameter_update), sub_params, &update); /* update parameters */ parameters_update(&ekf_param_handles, &ekf_params); } /* only run filter if sensor values changed */ if (fds[0].revents & POLLIN) { /* get latest measurements */ orb_copy(ORB_ID(sensor_combined), sub_raw, &raw); bool gps_updated; orb_check(sub_gps, &gps_updated); if (gps_updated) { orb_copy(ORB_ID(vehicle_gps_position), sub_gps, &gps); if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp) < 1000000) { mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f)); /* update mag declination rotation matrix */ R_decl.from_euler(0.0f, 0.0f, mag_decl); } } bool global_pos_updated; orb_check(sub_global_pos, &global_pos_updated); if (global_pos_updated) { orb_copy(ORB_ID(vehicle_global_position), sub_global_pos, &global_pos); } if (!initialized) { // XXX disabling init for now initialized = true; // gyro_offsets[0] += raw.gyro_rad_s[0]; // gyro_offsets[1] += raw.gyro_rad_s[1]; // gyro_offsets[2] += raw.gyro_rad_s[2]; // offset_count++; // if (hrt_absolute_time() - start_time > 3000000LL) { // initialized = true; // gyro_offsets[0] /= offset_count; // gyro_offsets[1] /= offset_count; // gyro_offsets[2] /= offset_count; // } } else { perf_begin(ekf_loop_perf); /* Calculate data time difference in seconds */ dt = (raw.timestamp - last_measurement) / 1000000.0f; last_measurement = raw.timestamp; uint8_t update_vect[3] = {0, 0, 0}; /* Fill in gyro measurements */ if (sensor_last_timestamp[0] != raw.gyro_timestamp[0]) { update_vect[0] = 1; // sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); sensor_last_timestamp[0] = raw.gyro_timestamp[0]; } z_k[0] = raw.gyro_rad_s[0] - gyro_offsets[0]; z_k[1] = raw.gyro_rad_s[1] - gyro_offsets[1]; z_k[2] = raw.gyro_rad_s[2] - gyro_offsets[2]; /* update accelerometer measurements */ if (sensor_last_timestamp[1] != raw.accelerometer_timestamp[0]) { update_vect[1] = 1; // sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); sensor_last_timestamp[1] = raw.accelerometer_timestamp[0]; } hrt_abstime vel_t = 0; bool vel_valid = false; if (gps.eph < 5.0f && global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) { vel_valid = true; if (global_pos_updated) { vel_t = global_pos.timestamp; vel(0) = global_pos.vel_n; vel(1) = global_pos.vel_e; vel(2) = global_pos.vel_d; } } if (vel_valid) { /* velocity is valid */ if (vel_t != 0) { /* velocity updated */ if (last_vel_t != 0 && vel_t != last_vel_t) { float vel_dt = (vel_t - last_vel_t) / 1000000.0f; /* calculate acceleration in body frame */ acc = R.transposed() * ((vel - vel_prev) / vel_dt); } last_vel_t = vel_t; vel_prev = vel; } } else { /* velocity is valid, reset acceleration */ acc.zero(); vel_prev.zero(); last_vel_t = 0; } z_k[3] = raw.accelerometer_m_s2[0] - acc(0); z_k[4] = raw.accelerometer_m_s2[1] - acc(1); z_k[5] = raw.accelerometer_m_s2[2] - acc(2); /* update magnetometer measurements */ if (sensor_last_timestamp[2] != raw.magnetometer_timestamp[0] && /* check that the mag vector is > 0 */ fabsf(sqrtf(raw.magnetometer_ga[0] * raw.magnetometer_ga[0] + raw.magnetometer_ga[1] * raw.magnetometer_ga[1] + raw.magnetometer_ga[2] * raw.magnetometer_ga[2])) > 0.1f) { update_vect[2] = 1; // sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); sensor_last_timestamp[2] = raw.magnetometer_timestamp[0]; } bool vision_updated = false; orb_check(vision_sub, &vision_updated); bool mocap_updated = false; orb_check(mocap_sub, &mocap_updated); if (vision_updated) { orb_copy(ORB_ID(vision_position_estimate), vision_sub, &vision); } if (mocap_updated) { orb_copy(ORB_ID(att_pos_mocap), mocap_sub, &mocap); } if (mocap.timestamp_boot > 0 && (hrt_elapsed_time(&mocap.timestamp_boot) < 500000)) { math::Quaternion q(mocap.q); math::Matrix<3, 3> Rmoc = q.to_dcm(); math::Vector<3> v(1.0f, 0.0f, 0.4f); math::Vector<3> vn = Rmoc.transposed() * v; //Rmoc is Rwr (robot respect to world) while v is respect to world. Hence Rmoc must be transposed having (Rwr)' * Vw // Rrw * Vw = vn. This way we have consistency z_k[6] = vn(0); z_k[7] = vn(1); z_k[8] = vn(2); }else if (vision.timestamp_boot > 0 && (hrt_elapsed_time(&vision.timestamp_boot) < 500000)) { math::Quaternion q(vision.q); math::Matrix<3, 3> Rvis = q.to_dcm(); math::Vector<3> v(1.0f, 0.0f, 0.4f); math::Vector<3> vn = Rvis.transposed() * v; //Rvis is Rwr (robot respect to world) while v is respect to world. Hence Rvis must be transposed having (Rwr)' * Vw // Rrw * Vw = vn. This way we have consistency z_k[6] = vn(0); z_k[7] = vn(1); z_k[8] = vn(2); } else { z_k[6] = raw.magnetometer_ga[0]; z_k[7] = raw.magnetometer_ga[1]; z_k[8] = raw.magnetometer_ga[2]; } static bool const_initialized = false; /* initialize with good values once we have a reasonable dt estimate */ if (!const_initialized && dt < 0.05f && dt > 0.001f) { dt = 0.005f; parameters_update(&ekf_param_handles, &ekf_params); /* update mag declination rotation matrix */ if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp) < 1000000) { mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f)); } /* update mag declination rotation matrix */ R_decl.from_euler(0.0f, 0.0f, mag_decl); x_aposteriori_k[0] = z_k[0]; x_aposteriori_k[1] = z_k[1]; x_aposteriori_k[2] = z_k[2]; x_aposteriori_k[3] = 0.0f; x_aposteriori_k[4] = 0.0f; x_aposteriori_k[5] = 0.0f; x_aposteriori_k[6] = z_k[3]; x_aposteriori_k[7] = z_k[4]; x_aposteriori_k[8] = z_k[5]; x_aposteriori_k[9] = z_k[6]; x_aposteriori_k[10] = z_k[7]; x_aposteriori_k[11] = z_k[8]; const_initialized = true; } /* do not execute the filter if not initialized */ if (!const_initialized) { continue; } /* Call the estimator */ AttitudeEKF(false, // approx_prediction (unsigned char)ekf_params.use_moment_inertia, update_vect, dt, z_k, ekf_params.q[0], // q_rotSpeed, ekf_params.q[1], // q_rotAcc ekf_params.q[2], // q_acc ekf_params.q[3], // q_mag ekf_params.r[0], // r_gyro ekf_params.r[1], // r_accel ekf_params.r[2], // r_mag ekf_params.moment_inertia_J, x_aposteriori, P_aposteriori, Rot_matrix, euler, debugOutput); /* swap values for next iteration, check for fatal inputs */ if (PX4_ISFINITE(euler[0]) && PX4_ISFINITE(euler[1]) && PX4_ISFINITE(euler[2])) { memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k)); memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k)); } else { /* due to inputs or numerical failure the output is invalid, skip it */ continue; } if (last_data > 0 && raw.timestamp - last_data > 30000) { warnx("sensor data missed! (%llu)\n", static_cast<unsigned long long>(raw.timestamp - last_data)); } last_data = raw.timestamp; /* send out */ att.timestamp = raw.timestamp; att.roll = euler[0]; att.pitch = euler[1]; att.yaw = euler[2] + mag_decl; att.rollspeed = x_aposteriori[0]; att.pitchspeed = x_aposteriori[1]; att.yawspeed = x_aposteriori[2]; att.rollacc = x_aposteriori[3]; att.pitchacc = x_aposteriori[4]; att.yawacc = x_aposteriori[5]; att.g_comp[0] = raw.accelerometer_m_s2[0] - acc(0); att.g_comp[1] = raw.accelerometer_m_s2[1] - acc(1); att.g_comp[2] = raw.accelerometer_m_s2[2] - acc(2); /* copy offsets */ memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets)); /* magnetic declination */ math::Matrix<3, 3> R_body = (&Rot_matrix[0]); R = R_decl * R_body; math::Quaternion q; q.from_dcm(R); /* copy rotation matrix */ memcpy(&att.R[0], &R.data[0][0], sizeof(att.R)); memcpy(&att.q[0],&q.data[0],sizeof(att.q)); att.R_valid = true; if (PX4_ISFINITE(att.q[0]) && PX4_ISFINITE(att.q[1]) && PX4_ISFINITE(att.q[2]) && PX4_ISFINITE(att.q[3])) { // Broadcast orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); } else { warnx("ERR: NaN estimate!"); } perf_end(ekf_loop_perf); } } } loopcounter++; } thread_running = false; return 0; }
void Delivery::to_destination() { // set a mission to destination with takeoff enabled // Status = enroute ; change to Dropoff at completion if (_complete) { // Update status now that travel to destination is complete and reset _first_run for next stage _first_run = true; mavlink_log_critical(_navigator->get_mavlink_fd(), "Black Hawk at Location"); advance_delivery(); return; } check_mission_valid(); /* check if anything has changed */ bool onboard_updated = false; orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated); if (onboard_updated) { update_onboard_mission(); } bool offboard_updated = false; orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated); if (offboard_updated) { update_offboard_mission(); } /* reset mission items if needed */ if (onboard_updated || offboard_updated) { set_mission_items(); } /* lets check if we reached the current mission item */ if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) { set_mission_item_reached(); if (_mission_item.autocontinue) { /* switch to next waypoint if 'autocontinue' flag set */ advance_mission(); set_mission_items(); mavlink_log_critical(_navigator->get_mavlink_fd(), "Black Hawk advancing mission"); } } else if (_mission_type != MISSION_TYPE_NONE &&_param_altmode.get() == MISSION_ALTMODE_FOH) { altitude_sp_foh_update(); } else { /* if waypoint position reached allow loiter on the setpoint */ if (_waypoint_position_reached && _mission_item.nav_cmd != NAV_CMD_IDLE) { _navigator->set_can_loiter_at_sp(true); _complete = true; } } /* see if we need to update the current yaw heading for rotary wing types */ if (_navigator->get_vstatus()->is_rotary_wing && _param_yawmode.get() != MISSION_YAWMODE_NONE && _mission_type != MISSION_TYPE_NONE) { heading_sp_update(); } }
void BlinkM::led() { static int vehicle_status_sub_fd; static int vehicle_gps_position_sub_fd; static int num_of_cells = 0; static int detected_cells_runcount = 0; static int t_led_color[8] = { 0, 0, 0, 0, 0, 0, 0, 0}; static int t_led_blink = 0; static int led_thread_runcount=0; static int led_interval = 1000; static int no_data_vehicle_status = 0; static int no_data_vehicle_gps_position = 0; static bool topic_initialized = false; static bool detected_cells_blinked = false; static bool led_thread_ready = true; int num_of_used_sats = 0; if(!topic_initialized) { vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status)); orb_set_interval(vehicle_status_sub_fd, 1000); vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position)); orb_set_interval(vehicle_gps_position_sub_fd, 1000); topic_initialized = true; } if(led_thread_ready == true) { if(!detected_cells_blinked) { if(num_of_cells > 0) { t_led_color[0] = LED_PURPLE; } if(num_of_cells > 1) { t_led_color[1] = LED_PURPLE; } if(num_of_cells > 2) { t_led_color[2] = LED_PURPLE; } if(num_of_cells > 3) { t_led_color[3] = LED_PURPLE; } if(num_of_cells > 4) { t_led_color[4] = LED_PURPLE; } t_led_color[5] = LED_OFF; t_led_color[6] = LED_OFF; t_led_color[7] = LED_OFF; t_led_blink = LED_BLINK; } else { t_led_color[0] = led_color_1; t_led_color[1] = led_color_2; t_led_color[2] = led_color_3; t_led_color[3] = led_color_4; t_led_color[4] = led_color_5; t_led_color[5] = led_color_6; t_led_color[6] = led_color_7; t_led_color[7] = led_color_8; t_led_blink = led_blink; } led_thread_ready = false; } if (led_thread_runcount & 1) { if (t_led_blink) setLEDColor(LED_OFF); led_interval = LED_OFFTIME; } else { setLEDColor(t_led_color[(led_thread_runcount / 2) % 8]); //led_interval = (led_thread_runcount & 1) : LED_ONTIME; led_interval = LED_ONTIME; } if (led_thread_runcount == 15) { /* obtained data for the first file descriptor */ struct vehicle_status_s vehicle_status_raw; struct vehicle_gps_position_s vehicle_gps_position_raw; memset(&vehicle_status_raw, 0, sizeof(vehicle_status_raw)); memset(&vehicle_gps_position_raw, 0, sizeof(vehicle_gps_position_raw)); bool new_data_vehicle_status; bool new_data_vehicle_gps_position; orb_check(vehicle_status_sub_fd, &new_data_vehicle_status); if (new_data_vehicle_status) { orb_copy(ORB_ID(vehicle_status), vehicle_status_sub_fd, &vehicle_status_raw); no_data_vehicle_status = 0; } else { no_data_vehicle_status++; if(no_data_vehicle_status >= 3) no_data_vehicle_status = 3; } orb_check(vehicle_gps_position_sub_fd, &new_data_vehicle_gps_position); if (new_data_vehicle_gps_position) { orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub_fd, &vehicle_gps_position_raw); no_data_vehicle_gps_position = 0; } else { no_data_vehicle_gps_position++; if(no_data_vehicle_gps_position >= 3) no_data_vehicle_gps_position = 3; } /* get number of used satellites in navigation */ num_of_used_sats = 0; //for(int satloop=0; satloop<20; satloop++) { for(int satloop=0; satloop<sizeof(vehicle_gps_position_raw.satellite_used); satloop++) { if(vehicle_gps_position_raw.satellite_used[satloop] == 1) { num_of_used_sats++; } } if(new_data_vehicle_status || no_data_vehicle_status < 3){ if(num_of_cells == 0) { /* looking for lipo cells that are connected */ printf("<blinkm> checking cells\n"); for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) { if(vehicle_status_raw.voltage_battery < num_of_cells * MAX_CELL_VOLTAGE) break; } printf("<blinkm> cells found:%u\n", num_of_cells); } else { if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { /* LED Pattern for battery low warning */ led_color_1 = LED_YELLOW; led_color_2 = LED_YELLOW; led_color_3 = LED_YELLOW; led_color_4 = LED_YELLOW; led_color_5 = LED_YELLOW; led_color_6 = LED_YELLOW; led_color_7 = LED_YELLOW; led_color_8 = LED_YELLOW; led_blink = LED_BLINK; } else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { /* LED Pattern for battery critical alerting */ led_color_1 = LED_RED; led_color_2 = LED_RED; led_color_3 = LED_RED; led_color_4 = LED_RED; led_color_5 = LED_RED; led_color_6 = LED_RED; led_color_7 = LED_RED; led_color_8 = LED_RED; led_blink = LED_BLINK; } else { /* no battery warnings here */ if(vehicle_status_raw.flag_system_armed == false) { /* system not armed */ led_color_1 = LED_RED; led_color_2 = LED_RED; led_color_3 = LED_RED; led_color_4 = LED_RED; led_color_5 = LED_RED; led_color_6 = LED_RED; led_color_7 = LED_RED; led_color_8 = LED_RED; led_blink = LED_NOBLINK; } else { /* armed system - initial led pattern */ led_color_1 = LED_RED; led_color_2 = LED_RED; led_color_3 = LED_RED; led_color_4 = LED_OFF; led_color_5 = LED_OFF; led_color_6 = LED_OFF; led_color_7 = LED_OFF; led_color_8 = LED_OFF; led_blink = LED_BLINK; /* handle 4th led - flightmode indicator */ switch((int)vehicle_status_raw.flight_mode) { case VEHICLE_FLIGHT_MODE_MANUAL: led_color_4 = LED_AMBER; break; case VEHICLE_FLIGHT_MODE_STAB: led_color_4 = LED_YELLOW; break; case VEHICLE_FLIGHT_MODE_HOLD: led_color_4 = LED_BLUE; break; case VEHICLE_FLIGHT_MODE_AUTO: led_color_4 = LED_GREEN; break; } if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) { /* handling used sat´s */ if(num_of_used_sats >= 7) { led_color_1 = LED_OFF; led_color_2 = LED_OFF; led_color_3 = LED_OFF; } else if(num_of_used_sats == 6) { led_color_2 = LED_OFF; led_color_3 = LED_OFF; } else if(num_of_used_sats == 5) { led_color_3 = LED_OFF; } } else { /* no vehicle_gps_position data */ led_color_1 = LED_WHITE; led_color_2 = LED_WHITE; led_color_3 = LED_WHITE; } } } } } else { /* LED Pattern for general Error - no vehicle_status can retrieved */ led_color_1 = LED_WHITE; led_color_2 = LED_WHITE; led_color_3 = LED_WHITE; led_color_4 = LED_WHITE; led_color_5 = LED_WHITE; led_color_6 = LED_WHITE; led_color_7 = LED_WHITE; led_color_8 = LED_WHITE; led_blink = LED_BLINK; } /* printf( "<blinkm> Volt:%8.4f\tArmed:%4u\tMode:%4u\tCells:%4u\tNDVS:%4u\tNDSAT:%4u\tSats:%4u\tFix:%4u\tVisible:%4u\n", vehicle_status_raw.voltage_battery, vehicle_status_raw.flag_system_armed, vehicle_status_raw.flight_mode, num_of_cells, no_data_vehicle_status, no_data_vehicle_gps_position, num_of_used_sats, vehicle_gps_position_raw.fix_type, vehicle_gps_position_raw.satellites_visible); */ led_thread_runcount=0; led_thread_ready = true; led_interval = LED_OFFTIME; if(detected_cells_runcount < 4){ detected_cells_runcount++; } else { detected_cells_blinked = true; } } else { led_thread_runcount++; } if(systemstate_run == true) { /* re-queue ourselves to run again later */ work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, led_interval); } else { stop_script(); set_rgb(0,0,0); } }
/* * EKF Attitude Estimator main function. * * Estimates the attitude recursively once started. * * @param argc number of commandline arguments (plus command name) * @param argv strings containing the arguments */ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) { const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds float dt = 0.005f; /* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */ float z_k[9] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 9.81f, 0.2f, -0.2f, 0.2f}; /**< Measurement vector */ float x_aposteriori_k[12]; /**< states */ float P_aposteriori_k[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 100.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f, }; /**< init: diagonal matrix with big values */ float x_aposteriori[12]; float P_aposteriori[144]; /* output euler angles */ float euler[3] = {0.0f, 0.0f, 0.0f}; float Rot_matrix[9] = {1.f, 0, 0, 0, 1.f, 0, 0, 0, 1.f }; /**< init: identity matrix */ // print text printf("Extended Kalman Filter Attitude Estimator initialized..\n\n"); fflush(stdout); int overloadcounter = 19; /* Initialize filter */ attitudeKalmanfilter_initialize(); /* store start time to guard against too slow update rates */ uint64_t last_run = hrt_absolute_time(); struct sensor_combined_s raw; memset(&raw, 0, sizeof(raw)); struct vehicle_gps_position_s gps; memset(&gps, 0, sizeof(gps)); struct vehicle_global_position_s global_pos; memset(&global_pos, 0, sizeof(global_pos)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); struct vehicle_control_mode_s control_mode; memset(&control_mode, 0, sizeof(control_mode)); struct vehicle_vicon_position_s vicon_pos; // Added by Ross Allen memset(&vicon_pos, 0, sizeof(vicon_pos)); uint64_t last_data = 0; uint64_t last_measurement = 0; uint64_t last_vel_t = 0; /* Vicon parameters - Added by Ross Allen */ bool vicon_valid = false; //~ static const hrt_abstime vicon_topic_timeout = 500000; // vicon topic timeout = 0.25s /* current velocity */ math::Vector<3> vel; vel.zero(); /* previous velocity */ math::Vector<3> vel_prev; vel_prev.zero(); /* actual acceleration (by GPS velocity) in body frame */ math::Vector<3> acc; acc.zero(); /* rotation matrix */ math::Matrix<3, 3> R; R.identity(); /* subscribe to raw data */ int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); /* rate-limit raw data updates to 333 Hz (sensors app publishes at 200, so this is just paranoid) */ orb_set_interval(sub_raw, 3); /* subscribe to GPS */ int sub_gps = orb_subscribe(ORB_ID(vehicle_gps_position)); /* subscribe to GPS */ int sub_global_pos = orb_subscribe(ORB_ID(vehicle_global_position)); /* subscribe to param changes */ int sub_params = orb_subscribe(ORB_ID(parameter_update)); /* subscribe to control mode*/ int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode)); /* subscribe to vicon position */ int vehicle_vicon_position_sub = orb_subscribe(ORB_ID(vehicle_vicon_position)); // Added by Ross Allen /* advertise attitude */ orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); int loopcounter = 0; thread_running = true; /* advertise debug value */ // struct debug_key_value_s dbg = { .key = "", .value = 0.0f }; // orb_advert_t pub_dbg = -1; /* keep track of sensor updates */ uint64_t sensor_last_timestamp[3] = {0, 0, 0}; struct attitude_estimator_ekf_params ekf_params; struct attitude_estimator_ekf_param_handles ekf_param_handles; /* initialize parameter handles */ parameters_init(&ekf_param_handles); bool initialized = false; float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f }; /* magnetic declination, in radians */ float mag_decl = 0.0f; /* rotation matrix for magnetic declination */ math::Matrix<3, 3> R_decl; R_decl.identity(); /* register the perf counter */ perf_counter_t ekf_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_ekf"); /* Main loop*/ while (!thread_should_exit) { struct pollfd fds[2]; fds[0].fd = sub_raw; fds[0].events = POLLIN; fds[1].fd = sub_params; fds[1].events = POLLIN; int ret = poll(fds, 2, 1000); /* Added by Ross Allen */ //~ hrt_abstime t = hrt_absolute_time(); bool updated; if (ret < 0) { /* XXX this is seriously bad - should be an emergency */ } else if (ret == 0) { /* check if we're in HIL - not getting sensor data is fine then */ orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode); if (!control_mode.flag_system_hil_enabled) { fprintf(stderr, "[att ekf] WARNING: Not getting sensors - sensor app running?\n"); } } else { /* only update parameters if they changed */ if (fds[1].revents & POLLIN) { /* read from param to clear updated flag */ struct parameter_update_s update; orb_copy(ORB_ID(parameter_update), sub_params, &update); /* update parameters */ parameters_update(&ekf_param_handles, &ekf_params); } /* only run filter if sensor values changed */ if (fds[0].revents & POLLIN) { /* get latest measurements */ orb_copy(ORB_ID(sensor_combined), sub_raw, &raw); bool gps_updated; orb_check(sub_gps, &gps_updated); if (gps_updated) { orb_copy(ORB_ID(vehicle_gps_position), sub_gps, &gps); if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) { mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f)); /* update mag declination rotation matrix */ R_decl.from_euler(0.0f, 0.0f, mag_decl); } } bool global_pos_updated; orb_check(sub_global_pos, &global_pos_updated); if (global_pos_updated) { orb_copy(ORB_ID(vehicle_global_position), sub_global_pos, &global_pos); } if (!initialized) { // XXX disabling init for now initialized = true; // gyro_offsets[0] += raw.gyro_rad_s[0]; // gyro_offsets[1] += raw.gyro_rad_s[1]; // gyro_offsets[2] += raw.gyro_rad_s[2]; // offset_count++; // if (hrt_absolute_time() - start_time > 3000000LL) { // initialized = true; // gyro_offsets[0] /= offset_count; // gyro_offsets[1] /= offset_count; // gyro_offsets[2] /= offset_count; // } } else { perf_begin(ekf_loop_perf); /* Calculate data time difference in seconds */ dt = (raw.timestamp - last_measurement) / 1000000.0f; last_measurement = raw.timestamp; uint8_t update_vect[3] = {0, 0, 0}; /* Fill in gyro measurements */ if (sensor_last_timestamp[0] != raw.timestamp) { update_vect[0] = 1; // sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); sensor_last_timestamp[0] = raw.timestamp; } z_k[0] = raw.gyro_rad_s[0] - gyro_offsets[0]; z_k[1] = raw.gyro_rad_s[1] - gyro_offsets[1]; z_k[2] = raw.gyro_rad_s[2] - gyro_offsets[2]; /* update accelerometer measurements */ if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) { update_vect[1] = 1; // sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); sensor_last_timestamp[1] = raw.accelerometer_timestamp; } hrt_abstime vel_t = 0; bool vel_valid = false; if (ekf_params.acc_comp == 1 && gps.fix_type >= 3 && gps.eph < 10.0f && gps.vel_ned_valid && hrt_absolute_time() < gps.timestamp_velocity + 500000) { vel_valid = true; if (gps_updated) { vel_t = gps.timestamp_velocity; vel(0) = gps.vel_n_m_s; vel(1) = gps.vel_e_m_s; vel(2) = gps.vel_d_m_s; } } else if (ekf_params.acc_comp == 2 && gps.eph < 5.0f && global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) { vel_valid = true; if (global_pos_updated) { vel_t = global_pos.timestamp; vel(0) = global_pos.vel_n; vel(1) = global_pos.vel_e; vel(2) = global_pos.vel_d; } } if (vel_valid) { /* velocity is valid */ if (vel_t != 0) { /* velocity updated */ if (last_vel_t != 0 && vel_t != last_vel_t) { float vel_dt = (vel_t - last_vel_t) / 1000000.0f; /* calculate acceleration in body frame */ acc = R.transposed() * ((vel - vel_prev) / vel_dt); } last_vel_t = vel_t; vel_prev = vel; } } else { /* velocity is valid, reset acceleration */ acc.zero(); vel_prev.zero(); last_vel_t = 0; } z_k[3] = raw.accelerometer_m_s2[0] - acc(0); z_k[4] = raw.accelerometer_m_s2[1] - acc(1); z_k[5] = raw.accelerometer_m_s2[2] - acc(2); /* update magnetometer measurements */ if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) { update_vect[2] = 1; // sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); sensor_last_timestamp[2] = raw.magnetometer_timestamp; } z_k[6] = raw.magnetometer_ga[0]; z_k[7] = raw.magnetometer_ga[1]; z_k[8] = raw.magnetometer_ga[2]; uint64_t now = hrt_absolute_time(); unsigned int time_elapsed = now - last_run; last_run = now; if (time_elapsed > loop_interval_alarm) { //TODO: add warning, cpu overload here // if (overloadcounter == 20) { // printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm); // overloadcounter = 0; // } overloadcounter++; } static bool const_initialized = false; /* initialize with good values once we have a reasonable dt estimate */ if (!const_initialized && dt < 0.05f && dt > 0.001f) { dt = 0.005f; parameters_update(&ekf_param_handles, &ekf_params); /* update mag declination rotation matrix */ if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) { mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f)); } else { mag_decl = ekf_params.mag_decl; } /* update mag declination rotation matrix */ R_decl.from_euler(0.0f, 0.0f, mag_decl); x_aposteriori_k[0] = z_k[0]; x_aposteriori_k[1] = z_k[1]; x_aposteriori_k[2] = z_k[2]; x_aposteriori_k[3] = 0.0f; x_aposteriori_k[4] = 0.0f; x_aposteriori_k[5] = 0.0f; x_aposteriori_k[6] = z_k[3]; x_aposteriori_k[7] = z_k[4]; x_aposteriori_k[8] = z_k[5]; x_aposteriori_k[9] = z_k[6]; x_aposteriori_k[10] = z_k[7]; x_aposteriori_k[11] = z_k[8]; const_initialized = true; } /* do not execute the filter if not initialized */ if (!const_initialized) { continue; } attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r, euler, Rot_matrix, x_aposteriori, P_aposteriori); /* swap values for next iteration, check for fatal inputs */ if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) { memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k)); memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k)); } else { /* due to inputs or numerical failure the output is invalid, skip it */ continue; } if (last_data > 0 && raw.timestamp - last_data > 30000) printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data); last_data = raw.timestamp; /* Check Vicon for attitude data*/ /* Added by Ross Allen */ orb_check(vehicle_vicon_position_sub, &updated); if (updated) { orb_copy(ORB_ID(vehicle_vicon_position), vehicle_vicon_position_sub, &vicon_pos); vicon_valid = vicon_pos.valid; } //~ if (vicon_valid && (t > (vicon_pos.timestamp + vicon_topic_timeout))) { //~ vicon_valid = false; //~ warnx("VICON timeout"); //~ } /* send out */ att.timestamp = raw.timestamp; att.roll = euler[0]; att.pitch = euler[1]; att.yaw = euler[2] + mag_decl; /* Use vicon yaw if valid and just overwrite*/ /* Added by Ross Allen */ if(vicon_valid) { att.yaw = vicon_pos.yaw; } att.rollspeed = x_aposteriori[0]; att.pitchspeed = x_aposteriori[1]; att.yawspeed = x_aposteriori[2]; att.rollacc = x_aposteriori[3]; att.pitchacc = x_aposteriori[4]; att.yawacc = x_aposteriori[5]; att.g_comp[0] = raw.accelerometer_m_s2[0] - acc(0); att.g_comp[1] = raw.accelerometer_m_s2[1] - acc(1); att.g_comp[2] = raw.accelerometer_m_s2[2] - acc(2); /* copy offsets */ memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets)); /* magnetic declination */ math::Matrix<3, 3> R_body = (&Rot_matrix[0]); R = R_decl * R_body; /* copy rotation matrix */ memcpy(&att.R[0][0], &R.data[0][0], sizeof(att.R)); att.R_valid = true; if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) { // Broadcast orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); } else { warnx("NaN in roll/pitch/yaw estimate!"); } perf_end(ekf_loop_perf); } } } loopcounter++; } thread_running = false; return 0; }
void BottleDrop::task_main() { _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); mavlink_log_info(_mavlink_fd, "[bottle_drop] started"); _command_sub = orb_subscribe(ORB_ID(vehicle_command)); _wind_estimate_sub = orb_subscribe(ORB_ID(wind_estimate)); bool updated = false; float z_0; // ground properties float turn_radius; // turn radius of the UAV float precision; // Expected precision of the UAV float ground_distance = _alt_clearance; // Replace by closer estimate in loop // constant float g = CONSTANTS_ONE_G; // constant of gravity [m/s^2] float m = 0.5f; // mass of bottle [kg] float rho = 1.2f; // air density [kg/m^3] float A = ((0.063f * 0.063f) / 4.0f * M_PI_F); // Bottle cross section [m^2] float dt_freefall_prediction = 0.01f; // step size of the free fall prediction [s] // Has to be estimated by experiment float cd = 0.86f; // Drag coefficient for a cylinder with a d/l ratio of 1/3 [] float t_signal = 0.084f; // Time span between sending the signal and the bottle top reaching level height with the bottom of the plane [s] float t_door = 0.7f; // The time the system needs to open the door + safety, is also the time the palyload needs to safely escape the shaft [s] // Definition float h_0; // height over target float az; // acceleration in z direction[m/s^2] float vz; // velocity in z direction [m/s] float z; // fallen distance [m] float h; // height over target [m] float ax; // acceleration in x direction [m/s^2] float vx; // ground speed in x direction [m/s] float x; // traveled distance in x direction [m] float vw; // wind speed [m/s] float vrx; // relative velocity in x direction [m/s] float v; // relative speed vector [m/s] float Fd; // Drag force [N] float Fdx; // Drag force in x direction [N] float Fdz; // Drag force in z direction [N] float x_drop, y_drop; // coordinates of the drop point in reference to the target (projection of NED) float x_t, y_t; // coordinates of the target in reference to the target x_t = 0, y_t = 0 (projection of NED) float x_l, y_l; // local position in projected coordinates float x_f, y_f; // to-be position of the UAV after dt_runs seconds in projected coordinates double x_f_NED, y_f_NED; // to-be position of the UAV after dt_runs seconds in NED float distance_open_door; // The distance the UAV travels during its doors open [m] float approach_error = 0.0f; // The error in radians between current ground vector and desired ground vector float distance_real = 0; // The distance between the UAVs position and the drop point [m] float future_distance = 0; // The distance between the UAVs to-be position and the drop point [m] unsigned counter = 0; param_t param_gproperties = param_find("BD_GPROPERTIES"); param_t param_turn_radius = param_find("BD_TURNRADIUS"); param_t param_precision = param_find("BD_PRECISION"); param_t param_cd = param_find("BD_OBJ_CD"); param_t param_mass = param_find("BD_OBJ_MASS"); param_t param_surface = param_find("BD_OBJ_SURFACE"); param_get(param_precision, &precision); param_get(param_turn_radius, &turn_radius); param_get(param_gproperties, &z_0); param_get(param_cd, &cd); param_get(param_mass, &m); param_get(param_surface, &A); int vehicle_global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); struct parameter_update_s update; memset(&update, 0, sizeof(update)); int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); struct mission_item_s flight_vector_s {}; struct mission_item_s flight_vector_e {}; flight_vector_s.nav_cmd = NAV_CMD_WAYPOINT; flight_vector_s.acceptance_radius = 50; // TODO: make parameter flight_vector_s.autocontinue = true; flight_vector_s.altitude_is_relative = false; flight_vector_e.nav_cmd = NAV_CMD_WAYPOINT; flight_vector_e.acceptance_radius = 50; // TODO: make parameter flight_vector_e.autocontinue = true; flight_vector_s.altitude_is_relative = false; struct wind_estimate_s wind; // wakeup source(s) struct pollfd fds[1]; // Setup of loop fds[0].fd = _command_sub; fds[0].events = POLLIN; // Whatever state the bay is in, we want it closed on startup lock_release(); close_bay(); while (!_task_should_exit) { /* wait for up to 100ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 50); /* 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; } /* vehicle commands updated */ if (fds[0].revents & POLLIN) { orb_copy(ORB_ID(vehicle_command), _command_sub, &_command); handle_command(&_command); } orb_check(vehicle_global_position_sub, &updated); if (updated) { /* copy global position */ orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &_global_pos); } if (_global_pos.timestamp == 0) { continue; } const unsigned sleeptime_us = 9500; hrt_abstime last_run = hrt_absolute_time(); float dt_runs = sleeptime_us / 1e6f; // switch to faster updates during the drop while (_drop_state > DROP_STATE_INIT) { // Get wind estimate orb_check(_wind_estimate_sub, &updated); if (updated) { orb_copy(ORB_ID(wind_estimate), _wind_estimate_sub, &wind); } // Get vehicle position orb_check(vehicle_global_position_sub, &updated); if (updated) { // copy global position orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &_global_pos); } // Get parameter updates orb_check(parameter_update_sub, &updated); if (updated) { // copy global position orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); // update all parameters param_get(param_gproperties, &z_0); param_get(param_turn_radius, &turn_radius); param_get(param_precision, &precision); } orb_check(_command_sub, &updated); if (updated) { orb_copy(ORB_ID(vehicle_command), _command_sub, &_command); handle_command(&_command); } float windspeed_norm = sqrtf(wind.windspeed_north * wind.windspeed_north + wind.windspeed_east * wind.windspeed_east); float groundspeed_body = sqrtf(_global_pos.vel_n * _global_pos.vel_n + _global_pos.vel_e * _global_pos.vel_e); ground_distance = _global_pos.alt - _target_position.alt; // Distance to drop position and angle error to approach vector // are relevant in all states greater than target valid (which calculates these positions) if (_drop_state > DROP_STATE_TARGET_VALID) { distance_real = fabsf(get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, _drop_position.lat, _drop_position.lon)); float ground_direction = atan2f(_global_pos.vel_e, _global_pos.vel_n); float approach_direction = get_bearing_to_next_waypoint(flight_vector_s.lat, flight_vector_s.lon, flight_vector_e.lat, flight_vector_e.lon); approach_error = _wrap_pi(ground_direction - approach_direction); if (counter % 90 == 0) { mavlink_log_info(_mavlink_fd, "drop distance %u, heading error %u", (unsigned)distance_real, (unsigned)math::degrees(approach_error)); } } switch (_drop_state) { case DROP_STATE_INIT: // do nothing break; case DROP_STATE_TARGET_VALID: { az = g; // acceleration in z direction[m/s^2] vz = 0; // velocity in z direction [m/s] z = 0; // fallen distance [m] h_0 = _global_pos.alt - _target_position.alt; // height over target at start[m] h = h_0; // height over target [m] ax = 0; // acceleration in x direction [m/s^2] vx = groundspeed_body;// XXX project // ground speed in x direction [m/s] x = 0; // traveled distance in x direction [m] vw = 0; // wind speed [m/s] vrx = 0; // relative velocity in x direction [m/s] v = groundspeed_body; // relative speed vector [m/s] Fd = 0; // Drag force [N] Fdx = 0; // Drag force in x direction [N] Fdz = 0; // Drag force in z direction [N] // Compute the distance the bottle will travel after it is dropped in body frame coordinates --> x while (h > 0.05f) { // z-direction vz = vz + az * dt_freefall_prediction; z = z + vz * dt_freefall_prediction; h = h_0 - z; // x-direction vw = windspeed_norm * logf(h / z_0) / logf(ground_distance / z_0); vx = vx + ax * dt_freefall_prediction; x = x + vx * dt_freefall_prediction; vrx = vx + vw; // drag force v = sqrtf(vz * vz + vrx * vrx); Fd = 0.5f * rho * A * cd * (v * v); Fdx = Fd * vrx / v; Fdz = Fd * vz / v; // acceleration az = g - Fdz / m; ax = -Fdx / m; } // compute drop vector x = groundspeed_body * t_signal + x; x_t = 0.0f; y_t = 0.0f; float wind_direction_n, wind_direction_e; if (windspeed_norm < 0.5f) { // If there is no wind, an arbitrarily direction is chosen wind_direction_n = 1.0f; wind_direction_e = 0.0f; } else { wind_direction_n = wind.windspeed_north / windspeed_norm; wind_direction_e = wind.windspeed_east / windspeed_norm; } x_drop = x_t + x * wind_direction_n; y_drop = y_t + x * wind_direction_e; map_projection_reproject(&ref, x_drop, y_drop, &_drop_position.lat, &_drop_position.lon); _drop_position.alt = _target_position.alt + _alt_clearance; // Compute flight vector map_projection_reproject(&ref, x_drop + 2 * turn_radius * wind_direction_n, y_drop + 2 * turn_radius * wind_direction_e, &(flight_vector_s.lat), &(flight_vector_s.lon)); flight_vector_s.altitude = _drop_position.alt; map_projection_reproject(&ref, x_drop - turn_radius * wind_direction_n, y_drop - turn_radius * wind_direction_e, &flight_vector_e.lat, &flight_vector_e.lon); flight_vector_e.altitude = _drop_position.alt; // Save WPs in datamanager const ssize_t len = sizeof(struct mission_item_s); if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 0, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_s, len) != len) { warnx("ERROR: could not save onboard WP"); } if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 1, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_e, len) != len) { warnx("ERROR: could not save onboard WP"); } _onboard_mission.count = 2; _onboard_mission.current_seq = 0; if (_onboard_mission_pub > 0) { orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); } else { _onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &_onboard_mission); } float approach_direction = get_bearing_to_next_waypoint(flight_vector_s.lat, flight_vector_s.lon, flight_vector_e.lat, flight_vector_e.lon); mavlink_log_critical(_mavlink_fd, "position set, approach heading: %u", (unsigned)distance_real, (unsigned)math::degrees(approach_direction + M_PI_F)); _drop_state = DROP_STATE_TARGET_SET; } break; case DROP_STATE_TARGET_SET: { float distance_wp2 = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, flight_vector_e.lat, flight_vector_e.lon); if (distance_wp2 < distance_real) { _onboard_mission.current_seq = 0; orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); } else { // We're close enough - open the bay distance_open_door = math::max(10.0f, 3.0f * fabsf(t_door * groundspeed_body)); if (isfinite(distance_real) && distance_real < distance_open_door && fabsf(approach_error) < math::radians(20.0f)) { open_bay(); _drop_state = DROP_STATE_BAY_OPEN; mavlink_log_info(_mavlink_fd, "#audio: opening bay"); } } } break; case DROP_STATE_BAY_OPEN: { if (_drop_approval) { map_projection_project(&ref, _global_pos.lat, _global_pos.lon, &x_l, &y_l); x_f = x_l + _global_pos.vel_n * dt_runs; y_f = y_l + _global_pos.vel_e * dt_runs; map_projection_reproject(&ref, x_f, y_f, &x_f_NED, &y_f_NED); future_distance = get_distance_to_next_waypoint(x_f_NED, y_f_NED, _drop_position.lat, _drop_position.lon); if (isfinite(distance_real) && (distance_real < precision) && ((distance_real < future_distance))) { drop(); _drop_state = DROP_STATE_DROPPED; mavlink_log_info(_mavlink_fd, "#audio: payload dropped"); } else { float distance_wp2 = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, flight_vector_e.lat, flight_vector_e.lon); if (distance_wp2 < distance_real) { _onboard_mission.current_seq = 0; orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); } } } } break; case DROP_STATE_DROPPED: /* 2s after drop, reset and close everything again */ if ((hrt_elapsed_time(&_doors_opened) > 2 * 1000 * 1000)) { _drop_state = DROP_STATE_INIT; _drop_approval = false; lock_release(); close_bay(); mavlink_log_info(_mavlink_fd, "#audio: closing bay"); // remove onboard mission _onboard_mission.current_seq = -1; _onboard_mission.count = 0; orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); } break; case DROP_STATE_BAY_CLOSED: // do nothing break; } counter++; // update_actuators(); // run at roughly 100 Hz usleep(sleeptime_us); dt_runs = hrt_elapsed_time(&last_run) / 1e6f; last_run = hrt_absolute_time(); } } warnx("exiting."); _main_task = -1; _exit(0); }
int quad_commander_thread_main(int argc, char *argv[]) { warnx("[quad_commander] has begun"); /** * Subscriptions */ struct quad_swarm_cmd_s swarm_cmd; struct quad_mode_s state; struct vehicle_status_s v_status; memset(&swarm_cmd, 0, sizeof(swarm_cmd)); memset(&state, 0, sizeof(state)); memset(&v_status, 0, sizeof(v_status)); int swarm_cmd_sub = 0; int state_sub = 0; int v_status_sub = 0; swarm_cmd_sub = orb_subscribe(ORB_ID(quad_swarm_cmd)); state_sub = orb_subscribe(ORB_ID(quad_mode)); v_status_sub = orb_subscribe(ORB_ID(vehicle_status)); /** * Topics to be published on */ struct quad_mode_s mode; orb_advert_t mode_pub = orb_advertise(ORB_ID(quad_mode), &mode); memset(&mode, 0, sizeof(mode)); param_t param_ptr = param_find("MAV_SYS_ID"); param_get(param_ptr, &system_id); mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); mavlink_log_info(mavlink_fd, "[QC%i] started", system_id); struct pollfd fd_cmd[1]; fd_cmd[0].fd = swarm_cmd_sub; fd_cmd[0].events = POLLIN; struct pollfd fd_state; fd_state.fd = state_sub; fd_state.events = POLLIN; /* Initial state of the quadrotor; operations always start from the ground */ state.current_state = QUAD_STATE_GROUNDED; mode.current_state = QUAD_STATE_GROUNDED; orb_publish(ORB_ID(quad_mode), mode_pub, &mode); bool transition_error = false; while ( !thread_should_exit ) { bool v_status_updated; orb_check(v_status_sub, &v_status_updated); if ( v_status_updated ) orb_copy(ORB_ID(vehicle_status), v_status_sub, &v_status); if ( v_status.arming_state == ARMING_STATE_STANDBY ) { state.current_state = QUAD_STATE_GROUNDED; mode.cmd = QUAD_CMD_PENDING; mode.current_state = QUAD_STATE_GROUNDED; orb_publish(ORB_ID(quad_mode), mode_pub, &mode); swarm_cmd.cmd_id = (float)0; } if ( (v_status.battery_warning == VEHICLE_BATTERY_WARNING_LOW) && (state.current_state != QUAD_STATE_GROUNDED) ) { low_battery = true; mavlink_log_critical(mavlink_fd, "[QC%i] Battery lavel low!", system_id); emergency_land( &state, &mode, &mode_pub, &state_sub, &swarm_cmd ); } else { low_battery = false; } int ret_state = poll(&fd_state, 1, 1); if ( ret_state < 0 ) { warnx("poll cmd error"); } else if ( ret_state == 0 ) { /* nothing happened */ } else if ( fd_state.revents & POLLIN ) { orb_copy(ORB_ID(quad_mode), state_sub, &state); } else { /* nothing happened */ } if ( state.error == true ) { int ret_value = emergency_land( &state, &mode, &mode_pub, &state_sub, &swarm_cmd ); mavlink_log_info(mavlink_fd, "[QC%i] emergency landing", system_id); error_msg( ret_value, &transition_error ); } int ret_cmd = poll(fd_cmd, 1, 250); if (ret_cmd < 0) { warnx("poll cmd error"); } else if (ret_cmd == 0) { /* no return value - nothing has happened */ } else if (fd_cmd[0].revents & POLLIN) { orb_copy(ORB_ID(quad_swarm_cmd), swarm_cmd_sub, &swarm_cmd); if ( swarm_cmd.cmd_id == (enum QUAD_MSG_CMD)QUAD_MSG_CMD_TAKEOFF ) { mavlink_log_info(mavlink_fd, "[QC%i] Takeoff transition begun!", system_id); int ret_value = take_off( &state, &mode, &mode_pub, &state_sub ); error_msg( ret_value, &transition_error ); } else if ( swarm_cmd.cmd_id == (enum QUAD_MSG_CMD)QUAD_MSG_CMD_LAND ) { mavlink_log_info(mavlink_fd, "[QC%i] Landing transition begun!", system_id); int ret_value = land( &state, &mode, &mode_pub, &state_sub, &transition_error ); error_msg( ret_value, &transition_error ); } else if ( swarm_cmd.cmd_id == (enum QUAD_MSG_CMD)QUAD_MSG_CMD_START_SWARM ) { mavlink_log_info(mavlink_fd, "[QC%i] Swarming transition begun!", system_id); int ret_value = start_swarm( &state, &mode, &mode_pub, &state_sub ); error_msg( ret_value, &transition_error ); } else if ( swarm_cmd.cmd_id == (enum QUAD_MSG_CMD)QUAD_MSG_CMD_STOP_SWARM ) { mavlink_log_info(mavlink_fd, "[QC%i] Stop swarming transition begun!", system_id); int ret_value = stop_swarm( &state, &mode, &mode_pub, &state_sub ); error_msg( ret_value, &transition_error ); } else { /* Nothing to do */ } } else { /* nothing happened */ } } }
void Navigator::task_main() { bool have_geofence_position_data = false; /* Try to load the geofence: * if /fs/microsd/etc/geofence.txt load from this file * else clear geofence data in datamanager */ struct stat buffer; if (stat(GEOFENCE_FILENAME, &buffer) == 0) { warnx("Try to load geofence.txt"); _geofence.loadFromFile(GEOFENCE_FILENAME); } else { if (_geofence.clearDm() != OK) { mavlink_log_critical(&_mavlink_log_pub, "failed clearing geofence"); } } /* do subscriptions */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); _fw_pos_ctrl_status_sub = orb_subscribe(ORB_ID(fw_pos_ctrl_status)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _home_pos_sub = orb_subscribe(ORB_ID(home_position)); _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); _param_update_sub = orb_subscribe(ORB_ID(parameter_update)); _vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command)); /* copy all topics first time */ vehicle_status_update(); vehicle_land_detected_update(); vehicle_control_mode_update(); global_position_update(); gps_position_update(); sensor_combined_update(); home_position_update(true); fw_pos_ctrl_status_update(); params_update(); /* wakeup source(s) */ px4_pollfd_struct_t fds[2] = {}; /* Setup of loop */ fds[0].fd = _global_pos_sub; fds[0].events = POLLIN; fds[1].fd = _vehicle_command_sub; fds[1].events = POLLIN; bool global_pos_available_once = false; while (!_task_should_exit) { /* wait for up to 200ms for data */ int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000); if (pret == 0) { /* timed out - periodic check for _task_should_exit, etc. */ if (global_pos_available_once) { global_pos_available_once = false; PX4_WARN("navigator: global position timeout"); } /* Let the loop run anyway, don't do `continue` here. */ } else if (pret < 0) { /* this is undesirable but not much we can do - might want to flag unhappy status */ PX4_WARN("nav: poll error %d, %d", pret, errno); continue; } else { /* success, global pos was available */ global_pos_available_once = true; } perf_begin(_loop_perf); bool updated; /* gps updated */ orb_check(_gps_pos_sub, &updated); if (updated) { gps_position_update(); if (_geofence.getSource() == Geofence::GF_SOURCE_GPS) { have_geofence_position_data = true; } } /* sensors combined updated */ orb_check(_sensor_combined_sub, &updated); if (updated) { sensor_combined_update(); } /* parameters updated */ orb_check(_param_update_sub, &updated); if (updated) { params_update(); updateParams(); } /* vehicle control mode updated */ orb_check(_control_mode_sub, &updated); if (updated) { vehicle_control_mode_update(); } /* vehicle status updated */ orb_check(_vstatus_sub, &updated); if (updated) { vehicle_status_update(); } /* vehicle land detected updated */ orb_check(_land_detected_sub, &updated); if (updated) { vehicle_land_detected_update(); } /* navigation capabilities updated */ orb_check(_fw_pos_ctrl_status_sub, &updated); if (updated) { fw_pos_ctrl_status_update(); } /* home position updated */ orb_check(_home_pos_sub, &updated); if (updated) { home_position_update(); } orb_check(_vehicle_command_sub, &updated); if (updated) { vehicle_command_s cmd; orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd); if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_REPOSITION) { struct position_setpoint_triplet_s *rep = get_reposition_triplet(); // store current position as previous position and goal as next rep->previous.yaw = get_global_position()->yaw; rep->previous.lat = get_global_position()->lat; rep->previous.lon = get_global_position()->lon; rep->previous.alt = get_global_position()->alt; rep->current.loiter_radius = get_loiter_radius(); rep->current.loiter_direction = 1; rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER; // Go on and check which changes had been requested if (PX4_ISFINITE(cmd.param4)) { rep->current.yaw = cmd.param4; } else { rep->current.yaw = NAN; } if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) { rep->current.lat = (cmd.param5 < 1000) ? cmd.param5 : cmd.param5 / (double)1e7; rep->current.lon = (cmd.param6 < 1000) ? cmd.param6 : cmd.param6 / (double)1e7; } else { rep->current.lat = get_global_position()->lat; rep->current.lon = get_global_position()->lon; } if (PX4_ISFINITE(cmd.param7)) { rep->current.alt = cmd.param7; } else { rep->current.alt = get_global_position()->alt; } rep->previous.valid = true; rep->current.valid = true; rep->next.valid = false; } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF) { struct position_setpoint_triplet_s *rep = get_takeoff_triplet(); // store current position as previous position and goal as next rep->previous.yaw = get_global_position()->yaw; rep->previous.lat = get_global_position()->lat; rep->previous.lon = get_global_position()->lon; rep->previous.alt = get_global_position()->alt; rep->current.loiter_radius = get_loiter_radius(); rep->current.loiter_direction = 1; rep->current.type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF; rep->current.yaw = cmd.param4; if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) { rep->current.lat = (cmd.param5 < 1000) ? cmd.param5 : cmd.param5 / (double)1e7; rep->current.lon = (cmd.param6 < 1000) ? cmd.param6 : cmd.param6 / (double)1e7; } else { // If one of them is non-finite, reset both rep->current.lat = NAN; rep->current.lon = NAN; } rep->current.alt = cmd.param7; rep->previous.valid = true; rep->current.valid = true; rep->next.valid = false; } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_PAUSE_CONTINUE) { warnx("navigator: got pause/continue command"); } } /* global position updated */ if (fds[0].revents & POLLIN) { global_position_update(); if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) { have_geofence_position_data = true; } } /* Check geofence violation */ static hrt_abstime last_geofence_check = 0; if (have_geofence_position_data && (_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) && (hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL)) { bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter, _home_pos, home_position_valid()); last_geofence_check = hrt_absolute_time(); have_geofence_position_data = false; _geofence_result.geofence_action = _geofence.getGeofenceAction(); if (!inside) { /* inform other apps via the mission result */ _geofence_result.geofence_violated = true; publish_geofence_result(); /* Issue a warning about the geofence violation once */ if (!_geofence_violation_warning_sent) { mavlink_log_critical(&_mavlink_log_pub, "Geofence violation"); _geofence_violation_warning_sent = true; } } else { /* inform other apps via the mission result */ _geofence_result.geofence_violated = false; publish_geofence_result(); /* Reset the _geofence_violation_warning_sent field */ _geofence_violation_warning_sent = false; } } /* Do stuff according to navigation state set by commander */ switch (_vstatus.nav_state) { case vehicle_status_s::NAVIGATION_STATE_MANUAL: case vehicle_status_s::NAVIGATION_STATE_ACRO: case vehicle_status_s::NAVIGATION_STATE_ALTCTL: case vehicle_status_s::NAVIGATION_STATE_POSCTL: case vehicle_status_s::NAVIGATION_STATE_TERMINATION: case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: _navigation_mode = nullptr; _can_loiter_at_sp = false; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: if (_fw_pos_ctrl_status.abort_landing) { // pos controller aborted landing, requests loiter // above landing waypoint _navigation_mode = &_loiter; _pos_sp_triplet_published_invalid_once = false; } else { _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_mission; } break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_loiter; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER: _pos_sp_triplet_published_invalid_once = false; if (_param_rcloss_act.get() == 1) { _navigation_mode = &_loiter; } else if (_param_rcloss_act.get() == 3) { _navigation_mode = &_land; } else if (_param_rcloss_act.get() == 4) { _navigation_mode = &_rcLoss; } else { /* if == 2 or unknown, RTL */ _navigation_mode = &_rtl; } break; case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_rtl; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_takeoff; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_land; break; case vehicle_status_s::NAVIGATION_STATE_DESCEND: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_land; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS: /* Use complex data link loss mode only when enabled via param * otherwise use rtl */ _pos_sp_triplet_published_invalid_once = false; if (_param_datalinkloss_act.get() == 1) { _navigation_mode = &_loiter; } else if (_param_datalinkloss_act.get() == 3) { _navigation_mode = &_land; } else if (_param_datalinkloss_act.get() == 4) { _navigation_mode = &_dataLinkLoss; } else { /* if == 2 or unknown, RTL */ _navigation_mode = &_rtl; } break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_engineFailure; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_gpsFailure; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_follow_target; break; default: _navigation_mode = nullptr; _can_loiter_at_sp = false; break; } /* iterate through navigation modes and set active/inactive for each */ for (unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) { _navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]); } /* if nothing is running, set position setpoint triplet invalid once */ if (_navigation_mode == nullptr && !_pos_sp_triplet_published_invalid_once) { _pos_sp_triplet_published_invalid_once = true; _pos_sp_triplet.previous.valid = false; _pos_sp_triplet.current.valid = false; _pos_sp_triplet.next.valid = false; _pos_sp_triplet_updated = true; } if (_pos_sp_triplet_updated) { publish_position_setpoint_triplet(); _pos_sp_triplet_updated = false; } if (_mission_result_updated) { publish_mission_result(); _mission_result_updated = false; } perf_end(_loop_perf); } warnx("exiting."); _navigator_task = -1; return; }
// update all estimator topics and write them to log file void Ekf2Replay::logIfUpdated() { bool updated = false; // update attitude struct vehicle_attitude_s att = {}; orb_copy(ORB_ID(vehicle_attitude), _att_sub, &att); // if the timestamp of the attitude is zero, then this means that the ekf did not // do an update so we can ignore this message and just continue if (att.timestamp == 0) { return; } memset(&log_message.body.att.q_w, 0, sizeof(log_ATT_s)); log_message.type = LOG_ATT_MSG; log_message.head1 = HEAD_BYTE1; log_message.head2 = HEAD_BYTE2; log_message.body.att.q_w = att.q[0]; log_message.body.att.q_x = att.q[1]; log_message.body.att.q_y = att.q[2]; log_message.body.att.q_z = att.q[3]; log_message.body.att.roll = atan2f(2 * (att.q[0] * att.q[1] + att.q[2] * att.q[3]), 1 - 2 * (att.q[1] * att.q[1] + att.q[2] * att.q[2])); log_message.body.att.pitch = asinf(2 * (att.q[0] * att.q[2] - att.q[3] * att.q[1])); log_message.body.att.yaw = atan2f(2 * (att.q[0] * att.q[3] + att.q[1] * att.q[2]), 1 - 2 * (att.q[2] * att.q[2] + att.q[3] * att.q[3])); log_message.body.att.roll_rate = att.rollspeed; log_message.body.att.pitch_rate = att.pitchspeed; log_message.body.att.yaw_rate = att.yawspeed; writeMessage(_write_fd, (void *)&log_message.head1, _formats[LOG_ATT_MSG].length); // update local position orb_check(_lpos_sub, &updated); if (updated) { struct vehicle_local_position_s lpos = {}; orb_copy(ORB_ID(vehicle_local_position), _lpos_sub, &lpos); log_message.type = LOG_LPOS_MSG; log_message.head1 = HEAD_BYTE1; log_message.head2 = HEAD_BYTE2; log_message.body.lpos.x = lpos.x; log_message.body.lpos.y = lpos.y; log_message.body.lpos.z = lpos.z; log_message.body.lpos.ground_dist = lpos.dist_bottom; log_message.body.lpos.ground_dist_rate = lpos.dist_bottom_rate; log_message.body.lpos.vx = lpos.vx; log_message.body.lpos.vy = lpos.vy; log_message.body.lpos.vz = lpos.vz; log_message.body.lpos.ref_lat = lpos.ref_lat * 1e7; log_message.body.lpos.ref_lon = lpos.ref_lon * 1e7; log_message.body.lpos.ref_alt = lpos.ref_alt; log_message.body.lpos.pos_flags = (lpos.xy_valid ? 1 : 0) | (lpos.z_valid ? 2 : 0) | (lpos.v_xy_valid ? 4 : 0) | (lpos.v_z_valid ? 8 : 0) | (lpos.xy_global ? 16 : 0) | (lpos.z_global ? 32 : 0); log_message.body.lpos.ground_dist_flags = (lpos.dist_bottom_valid ? 1 : 0); log_message.body.lpos.eph = lpos.eph; log_message.body.lpos.epv = lpos.epv; writeMessage(_write_fd, (void *)&log_message.head1, _formats[LOG_LPOS_MSG].length); } // update estimator status orb_check(_estimator_status_sub, &updated); if (updated) { struct estimator_status_s est_status = {}; orb_copy(ORB_ID(estimator_status), _estimator_status_sub, &est_status); unsigned maxcopy0 = (sizeof(est_status.states) < sizeof(log_message.body.est0.s)) ? sizeof(est_status.states) : sizeof( log_message.body.est0.s); log_message.type = LOG_EST0_MSG; log_message.head1 = HEAD_BYTE1; log_message.head2 = HEAD_BYTE2; memset(&(log_message.body.est0.s), 0, sizeof(log_message.body.est0)); memcpy(&(log_message.body.est0.s), est_status.states, maxcopy0); log_message.body.est0.n_states = est_status.n_states; log_message.body.est0.nan_flags = est_status.nan_flags; log_message.body.est0.fault_flags = est_status.filter_fault_flags; log_message.body.est0.timeout_flags = est_status.timeout_flags; writeMessage(_write_fd, (void *)&log_message.head1, _formats[LOG_EST0_MSG].length); log_message.type = LOG_EST1_MSG; log_message.head1 = HEAD_BYTE1; log_message.head2 = HEAD_BYTE2; unsigned maxcopy1 = ((sizeof(est_status.states) - maxcopy0) < sizeof(log_message.body.est1.s)) ? (sizeof( est_status.states) - maxcopy0) : sizeof(log_message.body.est1.s); memset(&(log_message.body.est1.s), 0, sizeof(log_message.body.est1.s)); memcpy(&(log_message.body.est1.s), ((char *)est_status.states) + maxcopy0, maxcopy1); writeMessage(_write_fd, (void *)&log_message.head1, _formats[LOG_EST1_MSG].length); log_message.type = LOG_EST2_MSG; log_message.head1 = HEAD_BYTE1; log_message.head2 = HEAD_BYTE2; unsigned maxcopy2 = (sizeof(est_status.covariances) < sizeof(log_message.body.est2.cov)) ? sizeof( est_status.covariances) : sizeof(log_message.body.est2.cov); memset(&(log_message.body.est2.cov), 0, sizeof(log_message.body.est2.cov)); memcpy(&(log_message.body.est2.cov), est_status.covariances, maxcopy2); writeMessage(_write_fd, (void *)&log_message.head1, _formats[LOG_EST2_MSG].length); log_message.type = LOG_EST3_MSG; log_message.head1 = HEAD_BYTE1; log_message.head2 = HEAD_BYTE2; unsigned maxcopy3 = ((sizeof(est_status.covariances) - maxcopy2) < sizeof(log_message.body.est3.cov)) ? (sizeof( est_status.covariances) - maxcopy2) : sizeof(log_message.body.est3.cov); memset(&(log_message.body.est3.cov), 0, sizeof(log_message.body.est3.cov)); memcpy(&(log_message.body.est3.cov), ((char *)est_status.covariances) + maxcopy2, maxcopy3); writeMessage(_write_fd, (void *)&log_message.head1, _formats[LOG_EST3_MSG].length); } // update ekf2 innovations orb_check(_innov_sub, &updated); if (updated) { struct ekf2_innovations_s innov = {}; orb_copy(ORB_ID(ekf2_innovations), _innov_sub, &innov); memset(&log_message.body.innov.s, 0, sizeof(log_message.body.innov.s)); log_message.type = LOG_EST4_MSG; log_message.head1 = HEAD_BYTE1; log_message.head2 = HEAD_BYTE2; for (unsigned i = 0; i < 6; i++) { log_message.body.innov.s[i] = innov.vel_pos_innov[i]; log_message.body.innov.s[i + 6] = innov.vel_pos_innov_var[i]; } for (unsigned i = 0; i < 3; i++) { log_message.body.innov.s[i + 12] = innov.output_tracking_error[i]; } writeMessage(_write_fd, (void *)&log_message.head1, _formats[LOG_EST4_MSG].length); log_message.type = LOG_EST5_MSG; log_message.head1 = HEAD_BYTE1; log_message.head2 = HEAD_BYTE2; memset(&(log_message.body.innov2.s), 0, sizeof(log_message.body.innov2.s)); for (unsigned i = 0; i < 3; i++) { log_message.body.innov2.s[i] = innov.mag_innov[i]; log_message.body.innov2.s[i + 3] = innov.mag_innov_var[i]; } log_message.body.innov2.s[6] = innov.heading_innov; log_message.body.innov2.s[7] = innov.heading_innov_var; log_message.body.innov2.s[8] = innov.airspeed_innov; log_message.body.innov2.s[9] = innov.airspeed_innov_var; writeMessage(_write_fd, (void *)&log_message.head1, _formats[LOG_EST5_MSG].length); // optical flow innovations and innovation variances log_message.type = LOG_EST6_MSG; log_message.head1 = HEAD_BYTE1; log_message.head2 = HEAD_BYTE2; memset(&(log_message.body.innov3.s), 0, sizeof(log_message.body.innov3.s)); for (unsigned i = 0; i < 2; i++) { log_message.body.innov3.s[i] = innov.flow_innov[i]; log_message.body.innov3.s[i + 2] = innov.flow_innov_var[i]; } log_message.body.innov3.s[4] = innov.hagl_innov; log_message.body.innov3.s[5] = innov.hagl_innov_var; writeMessage(_write_fd, (void *)&log_message.head1, _formats[LOG_EST6_MSG].length); } // update control state orb_check(_control_state_sub, &updated); if (updated) { struct control_state_s control_state = {}; orb_copy(ORB_ID(control_state), _control_state_sub, &control_state); log_message.type = LOG_CTS_MSG; log_message.head1 = HEAD_BYTE1; log_message.head2 = HEAD_BYTE2; log_message.body.control_state.vx_body = control_state.x_vel; log_message.body.control_state.vy_body = control_state.y_vel; log_message.body.control_state.vz_body = control_state.z_vel; log_message.body.control_state.airspeed = control_state.airspeed; log_message.body.control_state.roll_rate = control_state.roll_rate; log_message.body.control_state.pitch_rate = control_state.pitch_rate; log_message.body.control_state.yaw_rate = control_state.yaw_rate; writeMessage(_write_fd, (void *)&log_message.head1, _formats[LOG_CTS_MSG].length); } }
int nshterm_main(int argc, char *argv[]) { if (argc < 2) { printf("Usage: nshterm <device>\n"); exit(1); } unsigned retries = 0; int fd = -1; int armed_fd = orb_subscribe(ORB_ID(actuator_armed)); struct actuator_armed_s armed; /* we assume the system does not provide arming status feedback */ bool armed_updated = false; /* try the first 30 seconds or if arming system is ready */ while ((retries < 300) || armed_updated) { /* abort if an arming topic is published and system is armed */ bool updated = false; if (orb_check(armed_fd, &updated)) { /* the system is now providing arming status feedback. * instead of timing out, we resort to abort bringing * up the terminal. */ armed_updated = true; orb_copy(ORB_ID(actuator_armed), armed_fd, &armed); if (armed.armed) { /* this is not an error, but we are done */ exit(0); } } /* the retries are to cope with the behaviour of /dev/ttyACM0 */ /* which may not be ready immediately. */ fd = open(argv[1], O_RDWR); if (fd != -1) { break; } usleep(100000); retries++; } if (fd == -1) { perror(argv[1]); exit(1); } /* set up the serial port with output processing */ /* Try to set baud rate */ struct termios uart_config; int termios_state; /* Back up the original uart configuration to restore it after exit */ if ((termios_state = tcgetattr(fd, &uart_config)) < 0) { warnx("ERR get config %s: %d\n", argv[1], termios_state); close(fd); return -1; } /* Set ONLCR flag (which appends a CR for every LF) */ uart_config.c_oflag |= (ONLCR | OPOST/* | OCRNL*/); if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) { warnx("ERR set config %s\n", argv[1]); close(fd); return -1; } /* setup standard file descriptors */ close(0); close(1); close(2); dup2(fd, 0); dup2(fd, 1); dup2(fd, 2); nsh_consolemain(0, NULL); close(fd); return OK; }
void Mission::on_active() { check_mission_valid(false); /* check if anything has changed */ bool onboard_updated = false; orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated); if (onboard_updated) { update_onboard_mission(); } bool offboard_updated = false; orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated); if (offboard_updated) { update_offboard_mission(); } /* reset the current offboard mission if needed */ if (need_to_reset_mission(true)) { reset_offboard_mission(_offboard_mission); update_offboard_mission(); offboard_updated = true; } /* reset mission items if needed */ if (onboard_updated || offboard_updated) { set_mission_items(); } /* lets check if we reached the current mission item */ if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) { set_mission_item_reached(); if (_mission_item.autocontinue) { /* switch to next waypoint if 'autocontinue' flag set */ advance_mission(); set_mission_items(); } } else if (_mission_type != MISSION_TYPE_NONE && _param_altmode.get() == MISSION_ALTMODE_FOH) { altitude_sp_foh_update(); } else { /* if waypoint position reached allow loiter on the setpoint */ if (_waypoint_position_reached && _mission_item.nav_cmd != NAV_CMD_IDLE) { _navigator->set_can_loiter_at_sp(true); } } /* see if we need to update the current yaw heading */ if ((_param_yawmode.get() != MISSION_YAWMODE_NONE && _param_yawmode.get() < MISSION_YAWMODE_MAX && _mission_type != MISSION_TYPE_NONE) || _navigator->get_vstatus()->is_vtol) { heading_sp_update(); } /* check if landing needs to be aborted */ if ((_mission_item.nav_cmd == NAV_CMD_LAND) && (_navigator->abort_landing())) { do_abort_landing(); } }
void Gimbal::cycle() { if (!_initialized) { /* get subscription handles */ _vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command)); _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _vehicle_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); /* get a publication handle on actuator output topic */ struct actuator_controls_s zero_report; memset(&zero_report, 0, sizeof(zero_report)); zero_report.timestamp = hrt_absolute_time(); _actuator_controls_2_topic = orb_advertise(ORB_ID(actuator_controls_2), &zero_report); if (_actuator_controls_2_topic == nullptr) { warnx("advert err"); } _mount_mode = vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT; _initialized = true; } bool updated = false; perf_begin(_sample_perf); float roll = 0.0f; float pitch = 0.0f; float yaw = 0.0f; float out_mount_mode = 0.0f; /* Parameter update */ bool params_updated; orb_check(_params_sub, ¶ms_updated); if (params_updated) { struct parameter_update_s param_update; orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_update); // XXX: is this actually necessary? update_params(); } /* Control mode update */ bool vehicle_control_mode_updated; orb_check(_vehicle_control_mode_sub, &vehicle_control_mode_updated); if (vehicle_control_mode_updated) { orb_copy(ORB_ID(vehicle_control_mode), _vehicle_control_mode_sub, &_control_mode); } /* Check attitude */ struct vehicle_attitude_s att; orb_copy(ORB_ID(vehicle_attitude), _att_sub, &att); if (_attitude_compensation_roll) { roll = 1.0f / M_PI_F * -att.roll; updated = true; } if (_attitude_compensation_pitch) { pitch = 1.0f / M_PI_F * -att.pitch; updated = true; } if (_attitude_compensation_yaw) { yaw = 1.0f / M_PI_F * att.yaw; updated = true; } /* Check manual input */ bool manual_updated; orb_check(_manual_control_sub, &manual_updated); if (manual_updated) { orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sub, &_manual_control); /* only check manual input for mount mode when not in offboard and aux chan is configured */ if (!_control_mode.flag_control_offboard_enabled && _parameters.aux_mnt_chn > 0) { float aux = 0.0f; switch (_parameters.aux_mnt_chn) { case 1: aux = _manual_control.aux1; break; case 2: aux = _manual_control.aux2; break; case 3: aux = _manual_control.aux3; break; } if (aux < 0.0f && _mount_mode != vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT) { _mount_mode = vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT; updated = true; } if (aux > 0.0f && _mount_mode != vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL) { _mount_mode = vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL; updated = true; } } } /* Check command input */ struct vehicle_command_s cmd; bool cmd_updated; orb_check(_vehicle_command_sub, &cmd_updated); if (cmd_updated) { orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd); if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL || cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT) { _control_cmd = cmd; _control_cmd_set = true; } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE) { _config_cmd = cmd; _config_cmd_set = true; } } if (_config_cmd_set) { _config_cmd_set = false; _attitude_compensation_roll = (int)_config_cmd.param2 == 1; _attitude_compensation_pitch = (int)_config_cmd.param3 == 1; _attitude_compensation_yaw = (int)_config_cmd.param4 == 1; /* only check commanded mount mode when in offboard */ if (_control_mode.flag_control_offboard_enabled && fabsf(_config_cmd.param1 - _mount_mode) > FLT_EPSILON) { _mount_mode = int(_config_cmd.param1 + 0.5f); updated = true; } } if (_control_cmd_set) { unsigned mountMode = _control_cmd.param7; DEVICE_DEBUG("control_cmd: %d, mountMode %d | param1: %8.4f param2: %8.4f", _control_cmd.command, mountMode, (double)_control_cmd.param1, (double)_control_cmd.param2); if (_control_cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL && mountMode == vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) { /* Convert to range -1 ... 1, which corresponds to -180deg ... 180deg */ roll += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param1; pitch += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param2; yaw += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param3; updated = true; } if (_control_cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT && mountMode == vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) { float gimbalDirectionQuat[] = {_control_cmd.param1, _control_cmd.param2, _control_cmd.param3, _control_cmd.param4}; math::Vector<3> gimablDirectionEuler = math::Quaternion(gimbalDirectionQuat).to_dcm().to_euler(); roll += gimablDirectionEuler(0); pitch += gimablDirectionEuler(1); yaw += gimablDirectionEuler(2); updated = true; } } /* consider mount mode if parameter is set */ if (_parameters.use_mnt > 0) { switch (_mount_mode) { case vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT: out_mount_mode = -1.0f; roll = 0.0f; pitch = 0.0f; yaw = 0.0f; break; case vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL: case vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING: case vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING: case vehicle_command_s::VEHICLE_MOUNT_MODE_GPS_POINT: out_mount_mode = 1.0f; break; default: out_mount_mode = -1.0f; } } if (updated) { struct actuator_controls_s controls; // DEVICE_DEBUG("publishing | roll: %8.4f pitch: %8.4f yaw: %8.4f", (double)roll, (double)pitch, (double)yaw); /* fill in the final control values */ controls.timestamp = hrt_absolute_time(); controls.control[0] = roll; controls.control[1] = pitch; controls.control[2] = yaw; //controls.control[3] = ; // camera shutter controls.control[4] = out_mount_mode; /* publish it */ orb_publish(ORB_ID(actuator_controls_2), _actuator_controls_2_topic, &controls); } /* notify anyone waiting for data */ poll_notify(POLLIN); perf_end(_sample_perf); /* schedule a fresh cycle call when the measurement is done */ work_queue(LPWORK, &_work, (worker_t)&Gimbal::cycle_trampoline, this, USEC2TICK(GIMBAL_UPDATE_INTERVAL)); }
/** * Mission waypoint manager main function */ void *mission_waypoint_manager_thread_main(void* args) { absolute_time now; int updated; float turn_distance; /* initialize waypoint manager */ // ************************************* subscribe ****************************************** global_position_setpoint_pub = orb_advertise (ORB_ID(vehicle_global_position_setpoint)); if (global_position_setpoint_pub == -1) { fprintf (stderr, "Waypoint manager thread failed to advertise the vehicle_global_position_setpoint topic\n"); exit (-1); } global_position_set_triplet_pub = orb_advertise (ORB_ID(vehicle_global_position_set_triplet)); if (global_position_set_triplet_pub == -1) { fprintf (stderr, "Waypoint manager thread failed to advertise the vehicle_global_position_set_triplet topic\n"); exit (-1); } // ************************************* subscribe ****************************************** struct vehicle_global_position_s global_pos; orb_subscr_t global_pos_sub = orb_subscribe (ORB_ID(vehicle_global_position)); if (global_pos_sub == -1) { fprintf (stderr, "Waypoint manager thread failed to subscribe the vehicle_global_position topic\n"); exit (-1); } struct vehicle_control_flags_s control_flags; orb_subscr_t control_flags_sub = orb_subscribe(ORB_ID(vehicle_control_flags)); if (control_flags_sub < 0) { fprintf (stderr, "Waypoint manager thread failed to subscribe to vehicle_control_flags topic\n"); exit(-1); } struct navigation_capabilities_s nav_cap; orb_subscr_t nav_cap_sub = orb_subscribe (ORB_ID(navigation_capabilities)); if (nav_cap_sub == -1) { fprintf (stderr, "Waypoint manager thread failed to subscribe the navigation_capabilities topic\n"); exit (-1); } struct parameter_update_s params; orb_subscr_t param_sub = orb_subscribe (ORB_ID(parameter_update)); if (param_sub == -1) { fprintf (stderr, "Waypoint manager thread failed to subscribe the parameter_update topic\n"); exit (-1); } orb_subscr_t mission_sub = orb_subscribe (ORB_ID(mission)); if (mission_sub == -1) { fprintf (stderr, "Waypoint manager thread failed to subscribe the mission topic\n"); exit (-1); } orb_subscr_t home_sub = orb_subscribe (ORB_ID(home_position)); if (home_sub == -1) { fprintf (stderr, "Waypoint manager thread failed to subscribe the home_position topic\n"); exit (-1); } /* abort on a nonzero return value from the parameter init */ if (mission_waypoint_manager_params_init() != 0 || wp_manager_init (mission_sub, home_sub) != 0) { fprintf (stderr, "Waypoint manager exiting on startup due to an error\n"); exit (-1); } turn_distance = (is_rotary_wing)? mission_waypoint_manager_parameters.mr_turn_distance : mission_waypoint_manager_parameters.fw_turn_distance; /* welcome user */ fprintf (stdout, "Waypoint manager started\n"); fflush(stdout); /* waypoint main eventloop */ while (!_shutdown_all_systems) { // wait for the position estimation for a max of 500ms updated = orb_poll (ORB_ID(vehicle_global_position), global_pos_sub, 500000); if (updated < 0) { // that's undesiderable but there is not much we can do fprintf (stderr, "Waypoint manager thread experienced an error waiting for actuator_controls topic\n"); continue; } else if (updated == 0) { continue; } else orb_copy (ORB_ID(vehicle_global_position), global_pos_sub, (void *) &global_pos); updated = orb_check (ORB_ID(vehicle_control_flags), control_flags_sub); if (updated) { orb_copy(ORB_ID(vehicle_control_flags), control_flags_sub, &control_flags); } updated = orb_check (ORB_ID(navigation_capabilities), nav_cap_sub); if (updated) { orb_copy (ORB_ID(navigation_capabilities), nav_cap_sub, (void *) &nav_cap); turn_distance = (nav_cap.turn_distance > 0)? nav_cap.turn_distance : turn_distance; } updated = orb_check (ORB_ID(parameter_update), param_sub); if (updated) { /* clear updated flag */ orb_copy(ORB_ID(parameter_update), param_sub, ¶ms); /* update multirotor_position_control_parameters */ mission_waypoint_manager_params_update(); } if (!control_flags.flag_control_manual_enabled && control_flags.flag_control_auto_enabled) { orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &g_sp); orb_publish(ORB_ID(vehicle_global_position_set_triplet), global_position_set_triplet_pub, &triplet); } now = get_absolute_time(); /* check if waypoint has been reached against the last positions */ check_waypoints_reached(now, &global_pos, turn_distance); /* sleep 25 ms */ usleep(25000); } /* * do unsubscriptions */ if (orb_unsubscribe (ORB_ID(vehicle_global_position), global_pos_sub, pthread_self()) < 0) fprintf (stderr, "Waypoint manager thread failed to unsubscribe to vehicle_global_position topic\n"); if (orb_unsubscribe(ORB_ID(vehicle_control_flags), control_flags_sub, pthread_self()) < 0) fprintf (stderr, "Waypoint manager thread failed to unsubscribe to vehicle_control_flags topic\n"); if (orb_unsubscribe (ORB_ID(navigation_capabilities), nav_cap_sub, pthread_self()) < 0) fprintf (stderr, "Waypoint manager thread failed to unsubscribe to navigation_capabilities topic\n"); if (orb_unsubscribe (ORB_ID(parameter_update), param_sub, pthread_self()) < 0) fprintf (stderr, "Waypoint manager thread failed to unsubscribe to parameter_update topic\n"); if (orb_unsubscribe (ORB_ID(mission), mission_sub, pthread_self()) < 0) fprintf (stderr, "Waypoint manager thread failed to unsubscribe to mission topic\n"); if (orb_unsubscribe (ORB_ID(home_position), home_sub, pthread_self()) < 0) fprintf (stderr, "Waypoint manager thread failed to unsubscribe to home_position topic\n"); /* * do unadvertises */ if (orb_unadvertise (ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, pthread_self()) < 0) fprintf (stderr, "Waypoint manager thread failed to unadvertise the global_position_setpoint_pub topic\n"); if (orb_unadvertise (ORB_ID(vehicle_global_position_set_triplet), global_position_set_triplet_pub, pthread_self()) < 0) fprintf (stderr, "Waypoint manager thread failed to unadvertise the vehicle_global_position_set_triplet topic\n"); return 0; }
int test_ppm_loopback(int argc, char *argv[]) { int _rc_sub = orb_subscribe(ORB_ID(input_rc)); int servo_fd, result; servo_position_t pos; servo_fd = open(PWM_OUTPUT0_DEVICE_PATH, O_RDWR); if (servo_fd < 0) { printf("failed opening /dev/pwm_servo\n"); } printf("Servo readback, pairs of values should match defaults\n"); unsigned servo_count; result = ioctl(servo_fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); if (result != OK) { warnx("PWM_SERVO_GET_COUNT"); (void)close(servo_fd); return ERROR; } for (unsigned i = 0; i < servo_count; i++) { result = ioctl(servo_fd, PWM_SERVO_GET(i), (unsigned long)&pos); if (result < 0) { printf("failed reading channel %u\n", i); } //printf("%u: %u %u\n", i, pos, data[i]); } // /* tell safety that its ok to disable it with the switch */ // result = ioctl(servo_fd, PWM_SERVO_SET_ARM_OK, 0); // if (result != OK) // warnx("FAIL: PWM_SERVO_SET_ARM_OK"); // tell output device that the system is armed (it will output values if safety is off) // result = ioctl(servo_fd, PWM_SERVO_ARM, 0); // if (result != OK) // warnx("FAIL: PWM_SERVO_ARM"); int pwm_values[] = {1200, 1300, 1900, 1700, 1500, 1250, 1800, 1400}; for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) { result = ioctl(servo_fd, PWM_SERVO_SET(i), pwm_values[i]); if (result) { (void)close(servo_fd); return ERROR; } else { warnx("channel %d set to %d", i, pwm_values[i]); } } warnx("servo count: %d", servo_count); struct pwm_output_values pwm_out = {.values = {0}, .channel_count = 0}; for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) { pwm_out.values[i] = pwm_values[i]; //warnx("channel %d: disarmed PWM: %d", i+1, pwm_values[i]); pwm_out.channel_count++; } result = ioctl(servo_fd, PWM_SERVO_SET_DISARMED_PWM, (long unsigned int)&pwm_out); /* give driver 10 ms to propagate */ /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */ struct input_rc_s rc_input; orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); usleep(100000); /* open PPM input and expect values close to the output values */ bool rc_updated; orb_check(_rc_sub, &rc_updated); if (rc_updated) { orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); // int ppm_fd = open(RC_INPUT_DEVICE_PATH, O_RDONLY); // struct input_rc_s rc; // result = read(ppm_fd, &rc, sizeof(rc)); // if (result != sizeof(rc)) { // warnx("Error reading RC output"); // (void)close(servo_fd); // (void)close(ppm_fd); // return ERROR; // } /* go and check values */ for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) { if (abs(rc_input.values[i] - pwm_values[i]) > 10) { warnx("comparison fail: RC: %d, expected: %d", rc_input.values[i], pwm_values[i]); (void)close(servo_fd); return ERROR; } } } else { warnx("failed reading RC input data"); (void)close(servo_fd); return ERROR; } close(servo_fd); warnx("PPM LOOPBACK TEST PASSED SUCCESSFULLY!"); return 0; }
int uORBTest::UnitTest::test_single() { test_note("try single-topic support"); struct orb_test t, u; int sfd; orb_advert_t ptopic; bool updated; t.val = 0; ptopic = orb_advertise(ORB_ID(orb_test), &t); if (ptopic == nullptr) { return test_fail("advertise failed: %d", errno); } test_note("publish handle 0x%08x", ptopic); sfd = orb_subscribe(ORB_ID(orb_test)); if (sfd < 0) { return test_fail("subscribe failed: %d", errno); } test_note("subscribe fd %d", sfd); u.val = 1; if (PX4_OK != orb_copy(ORB_ID(orb_test), sfd, &u)) { return test_fail("copy(1) failed: %d", errno); } if (u.val != t.val) { return test_fail("copy(1) mismatch: %d expected %d", u.val, t.val); } if (PX4_OK != orb_check(sfd, &updated)) { return test_fail("check(1) failed"); } if (updated) { return test_fail("spurious updated flag"); } t.val = 2; test_note("try publish"); if (PX4_OK != orb_publish(ORB_ID(orb_test), ptopic, &t)) { return test_fail("publish failed"); } if (PX4_OK != orb_check(sfd, &updated)) { return test_fail("check(2) failed"); } if (!updated) { return test_fail("missing updated flag"); } if (PX4_OK != orb_copy(ORB_ID(orb_test), sfd, &u)) { return test_fail("copy(2) failed: %d", errno); } if (u.val != t.val) { return test_fail("copy(2) mismatch: %d expected %d", u.val, t.val); } orb_unsubscribe(sfd); int ret = orb_unadvertise(ptopic); if (ret != PX4_OK) { return test_fail("orb_unadvertise failed: %i", ret); } return test_note("PASS single-topic test"); }
void Gimbal::cycle() { if (!_initialized) { /* get a subscription handle on the vehicle command topic */ _vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command)); /* get a publication handle on actuator output topic */ struct actuator_controls_s zero_report; memset(&zero_report, 0, sizeof(zero_report)); zero_report.timestamp = hrt_absolute_time(); _actuator_controls_2_topic = orb_advertise(ORB_ID(actuator_controls_2), &zero_report); if (_actuator_controls_2_topic < 0) { warnx("advert err"); } _initialized = true; } bool updated = false; perf_begin(_sample_perf); float roll = 0.0f; float pitch = 0.0f; float yaw = 0.0f; if (_attitude_compensation) { if (_att_sub < 0) { _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); } vehicle_attitude_s att; orb_copy(ORB_ID(vehicle_attitude), _att_sub, &att); roll = -att.roll; pitch = -att.pitch; yaw = att.yaw; updated = true; } struct vehicle_command_s cmd; bool cmd_updated; orb_check(_vehicle_command_sub, &cmd_updated); if (cmd_updated) { orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd); VEHICLE_MOUNT_MODE mountMode = (VEHICLE_MOUNT_MODE)cmd.param7; debug("cmd: %d, mountMode %d | param1: %8.4f param2: %8.4f", cmd.command, mountMode, (double)cmd.param1, (double)cmd.param2); if (cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL && mountMode == VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) { /* Convert to range -1 ... 1, which corresponds to -180deg ... 180deg */ roll += 1.0f / M_PI_F * M_DEG_TO_RAD_F * cmd.param1; pitch += 1.0f / M_PI_F * M_DEG_TO_RAD_F * cmd.param2; yaw += 1.0f / M_PI_F * M_DEG_TO_RAD_F * cmd.param3; updated = true; } if (cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT && mountMode == VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) { float gimbalDirectionQuat[] = {cmd.param1, cmd.param2, cmd.param3, cmd.param4}; math::Vector<3> gimablDirectionEuler = math::Quaternion(gimbalDirectionQuat).to_dcm().to_euler(); roll += gimablDirectionEuler(0); pitch += gimablDirectionEuler(1); yaw += gimablDirectionEuler(2); updated = true; } } if (updated) { struct actuator_controls_s controls; // debug("publishing | roll: %8.4f pitch: %8.4f yaw: %8.4f", (double)roll, (double)pitch, (double)yaw); /* fill in the final control values */ controls.timestamp = hrt_absolute_time(); controls.control[0] = roll; controls.control[1] = pitch; controls.control[2] = yaw; /* publish it */ orb_publish(ORB_ID(actuator_controls_2), _actuator_controls_2_topic, &controls); } /* notify anyone waiting for data */ poll_notify(POLLIN); perf_end(_sample_perf); /* schedule a fresh cycle call when the measurement is done */ work_queue(LPWORK, &_work, (worker_t)&Gimbal::cycle_trampoline, this, USEC2TICK(GIMBAL_UPDATE_INTERVAL)); }
int uORBTest::UnitTest::test_multi2() { test_note("Testing multi-topic 2 test (queue simulation)"); //test: first subscribe, then advertise _thread_should_exit = false; const int num_instances = 3; int orb_data_fd[num_instances]; int orb_data_next = 0; for (int i = 0; i < num_instances; ++i) { // PX4_WARN("subscribe %i, t=%" PRIu64, i, hrt_absolute_time()); orb_data_fd[i] = orb_subscribe_multi(ORB_ID(orb_test_medium_multi), i); } char *const args[1] = { nullptr }; int pubsub_task = px4_task_spawn_cmd("uorb_test_multi", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 1500, (px4_main_t)&uORBTest::UnitTest::pub_test_multi2_entry, args); if (pubsub_task < 0) { return test_fail("failed launching task"); } hrt_abstime last_time = 0; while (!_thread_should_exit) { bool updated = false; int orb_data_cur_fd = orb_data_fd[orb_data_next]; orb_check(orb_data_cur_fd, &updated); if (updated) { struct orb_test_medium msg; orb_copy(ORB_ID(orb_test_medium_multi), orb_data_cur_fd, &msg); // Relax timing requirement for Darwin CI system #ifdef __PX4_DARWIN usleep(10000); #else usleep(1000); #endif if (last_time >= msg.time && last_time != 0) { return test_fail("Timestamp not increasing! (%" PRIu64 " >= %" PRIu64 ")", last_time, msg.time); } last_time = msg.time; // PX4_WARN(" got message (val=%i, idx=%i, t=%" PRIu64 ")", msg.val, orb_data_next, msg.time); orb_data_next = (orb_data_next + 1) % num_instances; } } for (int i = 0; i < num_instances; ++i) { orb_unsubscribe(orb_data_fd[i]); } return test_note("PASS multi-topic 2 test (queue simulation)"); }
int UavcanNode::run() { (void)pthread_mutex_lock(&_node_mutex); // XXX figure out the output count _output_count = 2; _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); _test_motor_sub = orb_subscribe(ORB_ID(test_motor)); _actuator_direct_sub = orb_subscribe(ORB_ID(actuator_direct)); memset(&_outputs, 0, sizeof(_outputs)); /* * Set up the time synchronization */ const int slave_init_res = _time_sync_slave.start(); if (slave_init_res < 0) { warnx("Failed to start time_sync_slave"); _task_should_exit = true; } /* When we have a system wide notion of time update (i.e the transition from the initial * System RTC setting to the GPS) we would call uavcan_stm32::clock::setUtc() when that * happens, but for now we use adjustUtc with a correction of the hrt so that the * time bases are the same */ uavcan_stm32::clock::adjustUtc(uavcan::UtcDuration::fromUSec(hrt_absolute_time())); _master_timer.setCallback(TimerCallback(this, &UavcanNode::handle_time_sync)); _master_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000)); const int busevent_fd = ::open(uavcan_stm32::BusEvent::DevName, 0); if (busevent_fd < 0) { warnx("Failed to open %s", uavcan_stm32::BusEvent::DevName); _task_should_exit = true; } /* * XXX Mixing logic/subscriptions shall be moved into UavcanEscController::update(); * IO multiplexing shall be done here. */ _node.setModeOperational(); /* * This event is needed to wake up the thread on CAN bus activity (RX/TX/Error). * Please note that with such multiplexing it is no longer possible to rely only on * the value returned from poll() to detect whether actuator control has timed out or not. * Instead, all ORB events need to be checked individually (see below). */ add_poll_fd(busevent_fd); /* * setup poll to look for actuator direct input if we are * subscribed to the topic */ if (_actuator_direct_sub != -1) { _actuator_direct_poll_fd_num = add_poll_fd(_actuator_direct_sub); } while (!_task_should_exit) { switch (_fw_server_action) { case Start: _fw_server_status = start_fw_server(); break; case Stop: _fw_server_status = stop_fw_server(); break; case CheckFW: _fw_server_status = request_fw_check(); break; case None: default: break; } // update actuator controls subscriptions if needed if (_groups_subscribed != _groups_required) { subscribe(); _groups_subscribed = _groups_required; } // Mutex is unlocked while the thread is blocked on IO multiplexing (void)pthread_mutex_unlock(&_node_mutex); perf_end(_perfcnt_esc_mixer_total_elapsed); // end goes first, it's not a mistake const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs); perf_begin(_perfcnt_esc_mixer_total_elapsed); (void)pthread_mutex_lock(&_node_mutex); node_spin_once(); // Non-blocking bool new_output = false; // this would be bad... if (poll_ret < 0) { DEVICE_LOG("poll error %d", errno); continue; } else { // get controls for required topics bool controls_updated = false; for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { if (_poll_fds[_poll_ids[i]].revents & POLLIN) { controls_updated = true; orb_copy(_control_topics[i], _control_subs[i], &_controls[i]); } } } /* see if we have any direct actuator updates */ if (_actuator_direct_sub != -1 && (_poll_fds[_actuator_direct_poll_fd_num].revents & POLLIN) && orb_copy(ORB_ID(actuator_direct), _actuator_direct_sub, &_actuator_direct) == OK && !_test_in_progress) { if (_actuator_direct.nvalues > actuator_outputs_s::NUM_ACTUATOR_OUTPUTS) { _actuator_direct.nvalues = actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; } memcpy(&_outputs.output[0], &_actuator_direct.values[0], _actuator_direct.nvalues * sizeof(float)); _outputs.noutputs = _actuator_direct.nvalues; new_output = true; } // can we mix? if (_test_in_progress) { memset(&_outputs, 0, sizeof(_outputs)); if (_test_motor.motor_number < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS) { _outputs.output[_test_motor.motor_number] = _test_motor.value * 2.0f - 1.0f; _outputs.noutputs = _test_motor.motor_number + 1; } new_output = true; } else if (controls_updated && (_mixers != nullptr)) { // XXX one output group has 8 outputs max, // but this driver could well serve multiple groups. unsigned num_outputs_max = 8; // Do mixing _outputs.noutputs = _mixers->mix(&_outputs.output[0], num_outputs_max, NULL); new_output = true; } } if (new_output) { // iterate actuators, checking for valid values for (uint8_t i = 0; i < _outputs.noutputs; i++) { // last resort: catch NaN, INF and out-of-band errors if (!isfinite(_outputs.output[i])) { /* * Value is NaN, INF or out of band - set to the minimum value. * This will be clearly visible on the servo status and will limit the risk of accidentally * spinning motors. It would be deadly in flight. */ _outputs.output[i] = -1.0f; } // never go below min if (_outputs.output[i] < -1.0f) { _outputs.output[i] = -1.0f; } // never go above max if (_outputs.output[i] > 1.0f) { _outputs.output[i] = 1.0f; } } // Output to the bus _outputs.timestamp = hrt_absolute_time(); perf_begin(_perfcnt_esc_mixer_output_elapsed); _esc_controller.update_outputs(_outputs.output, _outputs.noutputs); perf_end(_perfcnt_esc_mixer_output_elapsed); } // Check motor test state bool updated = false; orb_check(_test_motor_sub, &updated); if (updated) { orb_copy(ORB_ID(test_motor), _test_motor_sub, &_test_motor); // Update the test status and check that we're not locked down _test_in_progress = (_test_motor.value > 0); _esc_controller.arm_single_esc(_test_motor.motor_number, _test_in_progress); } // Check arming state orb_check(_armed_sub, &updated); if (updated) { orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); // Update the armed status and check that we're not locked down and motor // test is not running bool set_armed = _armed.armed && !_armed.lockdown && !_test_in_progress; arm_actuators(set_armed); } } (void)::close(busevent_fd); teardown(); warnx("exiting."); exit(0); }
int uORBTest::UnitTest::test_queue() { test_note("Testing orb queuing"); struct orb_test_medium t, u; int sfd; orb_advert_t ptopic; bool updated; sfd = orb_subscribe(ORB_ID(orb_test_medium_queue)); if (sfd < 0) { return test_fail("subscribe failed: %d", errno); } const unsigned int queue_size = 11; t.val = 0; ptopic = orb_advertise_queue(ORB_ID(orb_test_medium_queue), &t, queue_size); if (ptopic == nullptr) { return test_fail("advertise failed: %d", errno); } orb_check(sfd, &updated); if (!updated) { return test_fail("update flag not set"); } if (PX4_OK != orb_copy(ORB_ID(orb_test_medium_queue), sfd, &u)) { return test_fail("copy(1) failed: %d", errno); } if (u.val != t.val) { return test_fail("copy(1) mismatch: %d expected %d", u.val, t.val); } orb_check(sfd, &updated); if (updated) { return test_fail("spurious updated flag"); } #define CHECK_UPDATED(element) \ orb_check(sfd, &updated); \ if (!updated) { \ return test_fail("update flag not set, element %i", element); \ } #define CHECK_NOT_UPDATED(element) \ orb_check(sfd, &updated); \ if (updated) { \ return test_fail("update flag set, element %i", element); \ } #define CHECK_COPY(i_got, i_correct) \ orb_copy(ORB_ID(orb_test_medium_queue), sfd, &u); \ if (i_got != i_correct) { \ return test_fail("got wrong element from the queue (got %i, should be %i)", i_got, i_correct); \ } //no messages in the queue anymore test_note(" Testing to write some elements..."); for (unsigned int i = 0; i < queue_size - 2; ++i) { t.val = i; orb_publish(ORB_ID(orb_test_medium_queue), ptopic, &t); } for (unsigned int i = 0; i < queue_size - 2; ++i) { CHECK_UPDATED(i); CHECK_COPY(u.val, i); } CHECK_NOT_UPDATED(queue_size); test_note(" Testing overflow..."); int overflow_by = 3; for (unsigned int i = 0; i < queue_size + overflow_by; ++i) { t.val = i; orb_publish(ORB_ID(orb_test_medium_queue), ptopic, &t); } for (unsigned int i = 0; i < queue_size; ++i) { CHECK_UPDATED(i); CHECK_COPY(u.val, i + overflow_by); } CHECK_NOT_UPDATED(queue_size); test_note(" Testing underflow..."); for (unsigned int i = 0; i < queue_size; ++i) { CHECK_NOT_UPDATED(i); CHECK_COPY(u.val, queue_size + overflow_by - 1); } t.val = 943; orb_publish(ORB_ID(orb_test_medium_queue), ptopic, &t); CHECK_UPDATED(-1); CHECK_COPY(u.val, t.val); #undef CHECK_COPY #undef CHECK_UPDATED #undef CHECK_NOT_UPDATED orb_unadvertise(ptopic); return test_note("PASS orb queuing"); }