mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-17 11:13:35 +08:00
Merge branch 'beta' into offboard2
This commit is contained in:
@@ -153,6 +153,7 @@ static uint64_t last_print_mode_reject_time = 0;
|
||||
static bool on_usb_power = false;
|
||||
|
||||
static float takeoff_alt = 5.0f;
|
||||
static int parachute_enabled = 0;
|
||||
|
||||
static struct vehicle_status_s status;
|
||||
static struct actuator_armed_s armed;
|
||||
@@ -568,7 +569,9 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
|
||||
/* Flight termination */
|
||||
case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command
|
||||
|
||||
if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
|
||||
//XXX: to enable the parachute, a param needs to be set
|
||||
//xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
|
||||
if (armed->armed && cmd->param3 > 0.5 && parachute_enabled) {
|
||||
transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION);
|
||||
result = VEHICLE_CMD_RESULT_ACCEPTED;
|
||||
ret = true;
|
||||
@@ -620,6 +623,7 @@ int commander_thread_main(int argc, char *argv[])
|
||||
param_t _param_system_id = param_find("MAV_SYS_ID");
|
||||
param_t _param_component_id = param_find("MAV_COMP_ID");
|
||||
param_t _param_takeoff_alt = param_find("NAV_TAKEOFF_ALT");
|
||||
param_t _param_enable_parachute = param_find("NAV_PARACHUTE_EN");
|
||||
|
||||
/* welcome user */
|
||||
warnx("starting");
|
||||
@@ -866,10 +870,10 @@ int commander_thread_main(int argc, char *argv[])
|
||||
|
||||
/* re-check RC calibration */
|
||||
rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd));
|
||||
|
||||
/* navigation parameters */
|
||||
param_get(_param_takeoff_alt, &takeoff_alt);
|
||||
}
|
||||
/* navigation parameters */
|
||||
param_get(_param_takeoff_alt, &takeoff_alt);
|
||||
param_get(_param_enable_parachute, ¶chute_enabled);
|
||||
}
|
||||
|
||||
orb_check(sp_man_sub, &updated);
|
||||
@@ -1321,7 +1325,7 @@ int commander_thread_main(int argc, char *argv[])
|
||||
|
||||
// TODO remove this hack
|
||||
/* flight termination in manual mode if assisted switch is on easy position */
|
||||
if (!status.is_rotary_wing && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) {
|
||||
if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) {
|
||||
if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) {
|
||||
tune_positive();
|
||||
}
|
||||
|
||||
@@ -42,6 +42,8 @@
|
||||
#include <unistd.h>
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <dirent.h>
|
||||
#include <fcntl.h>
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
@@ -50,6 +52,7 @@
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/drv_device.h>
|
||||
#include <mavlink/mavlink_log.h>
|
||||
|
||||
#include "state_machine_helper.h"
|
||||
@@ -344,6 +347,33 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
|
||||
|
||||
mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
|
||||
valid_transition = true;
|
||||
|
||||
// Disable publication of all attached sensors
|
||||
|
||||
/* list directory */
|
||||
DIR *d;
|
||||
struct dirent *direntry;
|
||||
d = opendir("/dev");
|
||||
if (d) {
|
||||
|
||||
while ((direntry = readdir(d)) != NULL) {
|
||||
|
||||
int sensfd = ::open(direntry->d_name, 0);
|
||||
int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 0);
|
||||
close(sensfd);
|
||||
|
||||
printf("Disabling %s\n: %s", direntry->d_name, (!block_ret) ? "OK" : "FAIL");
|
||||
}
|
||||
|
||||
closedir(d);
|
||||
|
||||
warnx("directory listing ok (FS mounted and readable)");
|
||||
|
||||
} else {
|
||||
/* failed opening dir */
|
||||
warnx("FAILED LISTING DEVICE ROOT DIRECTORY");
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
@@ -236,7 +236,7 @@ private:
|
||||
float land_slope_length;
|
||||
float land_H1_virt;
|
||||
float land_flare_alt_relative;
|
||||
float land_thrust_lim_horizontal_distance;
|
||||
float land_thrust_lim_alt_relative;
|
||||
float land_heading_hold_horizontal_distance;
|
||||
|
||||
} _parameters; /**< local copies of interesting parameters */
|
||||
@@ -281,7 +281,7 @@ private:
|
||||
param_t land_slope_length;
|
||||
param_t land_H1_virt;
|
||||
param_t land_flare_alt_relative;
|
||||
param_t land_thrust_lim_horizontal_distance;
|
||||
param_t land_thrust_lim_alt_relative;
|
||||
param_t land_heading_hold_horizontal_distance;
|
||||
|
||||
} _parameter_handles; /**< handles for interesting parameters */
|
||||
@@ -434,7 +434,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
|
||||
_parameter_handles.land_slope_length = param_find("FW_LND_SLLR");
|
||||
_parameter_handles.land_H1_virt = param_find("FW_LND_HVIRT");
|
||||
_parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT");
|
||||
_parameter_handles.land_thrust_lim_horizontal_distance = param_find("FW_LND_TLDIST");
|
||||
_parameter_handles.land_thrust_lim_alt_relative = param_find("FW_LND_TLALT");
|
||||
_parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST");
|
||||
|
||||
_parameter_handles.time_const = param_find("FW_T_TIME_CONST");
|
||||
@@ -523,7 +523,7 @@ FixedwingPositionControl::parameters_update()
|
||||
param_get(_parameter_handles.land_slope_length, &(_parameters.land_slope_length));
|
||||
param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt));
|
||||
param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative));
|
||||
param_get(_parameter_handles.land_thrust_lim_horizontal_distance, &(_parameters.land_thrust_lim_horizontal_distance));
|
||||
param_get(_parameter_handles.land_thrust_lim_alt_relative, &(_parameters.land_thrust_lim_alt_relative));
|
||||
param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance));
|
||||
|
||||
_l1_control.set_l1_damping(_parameters.l1_damping);
|
||||
@@ -558,7 +558,7 @@ FixedwingPositionControl::parameters_update()
|
||||
}
|
||||
|
||||
/* Update the landing slope */
|
||||
landingslope.update(math::radians(_parameters.land_slope_angle), _parameters.land_flare_alt_relative, _parameters.land_thrust_lim_horizontal_distance, _parameters.land_H1_virt);
|
||||
landingslope.update(math::radians(_parameters.land_slope_angle), _parameters.land_flare_alt_relative, _parameters.land_thrust_lim_alt_relative, _parameters.land_H1_virt);
|
||||
|
||||
/* Update and publish the navigation capabilities */
|
||||
_nav_capabilities.landing_slope_angle_rad = landingslope.landing_slope_angle_rad();
|
||||
@@ -836,6 +836,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
||||
|
||||
} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
|
||||
|
||||
float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
|
||||
|
||||
/* Horizontal landing control */
|
||||
/* switch to heading hold for the last meters, continue heading hold after */
|
||||
float wp_distance = get_distance_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
|
||||
@@ -846,7 +848,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
||||
|
||||
if (!land_noreturn_horizontal) {//set target_bearing in first occurrence
|
||||
if (pos_sp_triplet.previous.valid) {
|
||||
target_bearing = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
|
||||
target_bearing = bearing_lastwp_currwp;
|
||||
} else {
|
||||
target_bearing = _att.yaw;
|
||||
}
|
||||
@@ -888,10 +890,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
||||
float airspeed_approach = 1.3f * _parameters.airspeed_min;
|
||||
|
||||
float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)) * _parameters.land_slope_length;
|
||||
float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt);//getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt, landing_slope_angle_rad, horizontal_slope_displacement);
|
||||
float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitude(wp_distance, _pos_sp_triplet.current.alt);//getLandingSlopeAbsoluteAltitude(wp_distance, _pos_sp_triplet.current.alt, landing_slope_angle_rad, horizontal_slope_displacement);
|
||||
|
||||
|
||||
float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt);
|
||||
float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
|
||||
float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt);
|
||||
|
||||
if ( (_global_pos.alt < _pos_sp_triplet.current.alt + landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out
|
||||
|
||||
@@ -903,7 +904,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
||||
/* kill the throttle if param requests it */
|
||||
throttle_max = _parameters.throttle_max;
|
||||
|
||||
if (wp_distance < landingslope.motor_lim_horizontal_distance() || land_motor_lim) {
|
||||
if (_global_pos.alt < _pos_sp_triplet.current.alt + landingslope.motor_lim_relative_alt() || land_motor_lim) {
|
||||
throttle_max = math::min(throttle_max, _parameters.throttle_land_max);
|
||||
if (!land_motor_lim) {
|
||||
land_motor_lim = true;
|
||||
@@ -912,7 +913,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
||||
|
||||
}
|
||||
|
||||
float flare_curve_alt = landingslope.getFlareCurveAltitude(wp_distance, _pos_sp_triplet.current.alt);
|
||||
float flare_curve_alt = landingslope.getFlareCurveAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt);
|
||||
|
||||
/* avoid climbout */
|
||||
if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground)
|
||||
|
||||
@@ -168,9 +168,9 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f);
|
||||
PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f);
|
||||
PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f);
|
||||
|
||||
PARAM_DEFINE_FLOAT(FW_LND_ANG, 10.0f);
|
||||
PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f);
|
||||
PARAM_DEFINE_FLOAT(FW_LND_SLLR, 0.9f);
|
||||
PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f);
|
||||
PARAM_DEFINE_FLOAT(FW_LND_FLALT, 15.0f);
|
||||
PARAM_DEFINE_FLOAT(FW_LND_TLDIST, 30.0f);
|
||||
PARAM_DEFINE_FLOAT(FW_LND_TLALT, 5.0f);
|
||||
PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f);
|
||||
|
||||
@@ -48,13 +48,13 @@
|
||||
|
||||
void Landingslope::update(float landing_slope_angle_rad,
|
||||
float flare_relative_alt,
|
||||
float motor_lim_horizontal_distance,
|
||||
float motor_lim_relative_alt,
|
||||
float H1_virt)
|
||||
{
|
||||
|
||||
_landing_slope_angle_rad = landing_slope_angle_rad;
|
||||
_flare_relative_alt = flare_relative_alt;
|
||||
_motor_lim_horizontal_distance = motor_lim_horizontal_distance;
|
||||
_motor_lim_relative_alt = motor_lim_relative_alt;
|
||||
_H1_virt = H1_virt;
|
||||
|
||||
calculateSlopeValues();
|
||||
@@ -74,9 +74,21 @@ float Landingslope::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_
|
||||
return Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, wp_altitude, _horizontal_slope_displacement, _landing_slope_angle_rad);
|
||||
}
|
||||
|
||||
float Landingslope::getFlareCurveAltitude(float wp_landing_distance, float wp_landing_altitude)
|
||||
float Landingslope::getLandingSlopeAbsoluteAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude)
|
||||
{
|
||||
return wp_landing_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance)/_flare_constant) - _H1_virt;
|
||||
|
||||
/* If airplane is in front of waypoint return slope altitude, else return waypoint altitude */
|
||||
if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f))
|
||||
return getLandingSlopeAbsoluteAltitude(wp_distance, wp_altitude);
|
||||
else
|
||||
return wp_altitude;
|
||||
}
|
||||
|
||||
float Landingslope::getFlareCurveAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_landing_altitude)
|
||||
{
|
||||
/* If airplane is in front of waypoint return flare curve altitude, else return waypoint altitude */
|
||||
if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f))
|
||||
return wp_landing_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance)/_flare_constant) - _H1_virt;
|
||||
else
|
||||
return wp_landing_altitude;
|
||||
}
|
||||
|
||||
|
||||
@@ -49,7 +49,7 @@ private:
|
||||
/* see Documentation/fw_landing.png for a plot of the landing slope */
|
||||
float _landing_slope_angle_rad; /**< phi in the plot */
|
||||
float _flare_relative_alt; /**< h_flare,rel in the plot */
|
||||
float _motor_lim_horizontal_distance;
|
||||
float _motor_lim_relative_alt;
|
||||
float _H1_virt; /**< H1 in the plot */
|
||||
float _H0; /**< h_flare,rel + H1 in the plot */
|
||||
float _d1; /**< d1 in the plot */
|
||||
@@ -63,7 +63,18 @@ public:
|
||||
Landingslope() {}
|
||||
~Landingslope() {}
|
||||
|
||||
float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_landing_altitude);
|
||||
/**
|
||||
*
|
||||
* @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
|
||||
*/
|
||||
float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude);
|
||||
|
||||
/**
|
||||
*
|
||||
* @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
|
||||
* Performs check if aircraft is in front of waypoint to avoid climbout
|
||||
*/
|
||||
float getLandingSlopeAbsoluteAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_landing_altitude);
|
||||
|
||||
/**
|
||||
*
|
||||
@@ -85,17 +96,17 @@ public:
|
||||
}
|
||||
|
||||
|
||||
float getFlareCurveAltitude(float wp_distance, float wp_altitude);
|
||||
float getFlareCurveAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude);
|
||||
|
||||
void update(float landing_slope_angle_rad,
|
||||
float flare_relative_alt,
|
||||
float motor_lim_horizontal_distance,
|
||||
float motor_lim_relative_alt,
|
||||
float H1_virt);
|
||||
|
||||
|
||||
inline float landing_slope_angle_rad() {return _landing_slope_angle_rad;}
|
||||
inline float flare_relative_alt() {return _flare_relative_alt;}
|
||||
inline float motor_lim_horizontal_distance() {return _motor_lim_horizontal_distance;}
|
||||
inline float motor_lim_relative_alt() {return _motor_lim_relative_alt;}
|
||||
inline float H1_virt() {return _H1_virt;}
|
||||
inline float H0() {return _H0;}
|
||||
inline float d1() {return _d1;}
|
||||
|
||||
@@ -381,7 +381,7 @@ handle_message(mavlink_message_t *msg)
|
||||
tstatus.rxerrors = rstatus.rxerrors;
|
||||
tstatus.fixed = rstatus.fixed;
|
||||
|
||||
if (telemetry_status_pub == 0) {
|
||||
if (telemetry_status_pub <= 0) {
|
||||
telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus);
|
||||
|
||||
} else {
|
||||
|
||||
@@ -1080,9 +1080,8 @@ Navigator::start_loiter()
|
||||
mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude");
|
||||
}
|
||||
|
||||
_pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER;
|
||||
}
|
||||
|
||||
_pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER;
|
||||
_pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius;
|
||||
_pos_sp_triplet.current.loiter_direction = 1;
|
||||
_pos_sp_triplet.previous.valid = false;
|
||||
|
||||
@@ -55,9 +55,10 @@
|
||||
|
||||
PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f);
|
||||
PARAM_DEFINE_FLOAT(NAV_ACCEPT_RAD, 10.0f);
|
||||
PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 100.0f);
|
||||
PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);
|
||||
PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0);
|
||||
PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f); // default TAKEOFF altitude
|
||||
PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f); // slow descend from this altitude when landing
|
||||
PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f); // min altitude for going home in RTL mode
|
||||
PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, -1.0f); // delay after descend before landing, if set to -1 the system will not land but loiter at NAV_LAND_ALT
|
||||
PARAM_DEFINE_INT32(NAV_PARACHUTE_EN, 0); // enable parachute deployment
|
||||
|
||||
@@ -527,13 +527,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
||||
if (gps.fix_type >= 3) {
|
||||
/* hysteresis for GPS quality */
|
||||
if (gps_valid) {
|
||||
if (gps.eph_m > 10.0f || gps.epv_m > 10.0f) {
|
||||
if (gps.eph_m > 10.0f || gps.epv_m > 20.0f) {
|
||||
gps_valid = false;
|
||||
mavlink_log_info(mavlink_fd, "[inav] GPS signal lost");
|
||||
}
|
||||
|
||||
} else {
|
||||
if (gps.eph_m < 5.0f && gps.epv_m < 5.0f) {
|
||||
if (gps.eph_m < 5.0f && gps.epv_m < 10.0f) {
|
||||
gps_valid = true;
|
||||
mavlink_log_info(mavlink_fd, "[inav] GPS signal found");
|
||||
}
|
||||
@@ -589,8 +589,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
||||
corr_gps[2][1] = 0.0f;
|
||||
}
|
||||
|
||||
w_gps_xy = 1.0f / fmaxf(1.0f, gps.eph_m);
|
||||
w_gps_z = 1.0f / fmaxf(1.0f, gps.epv_m);
|
||||
w_gps_xy = 2.0f / fmaxf(2.0f, gps.eph_m);
|
||||
w_gps_z = 4.0f / fmaxf(4.0f, gps.epv_m);
|
||||
}
|
||||
|
||||
} else {
|
||||
|
||||
@@ -40,7 +40,7 @@
|
||||
|
||||
#include "position_estimator_inav_params.h"
|
||||
|
||||
PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 1.0f);
|
||||
PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 0.5f);
|
||||
PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f);
|
||||
PARAM_DEFINE_FLOAT(INAV_W_Z_ACC, 20.0f);
|
||||
PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f);
|
||||
|
||||
@@ -82,6 +82,7 @@
|
||||
#include <uORB/topics/airspeed.h>
|
||||
#include <uORB/topics/rc_channels.h>
|
||||
#include <uORB/topics/esc_status.h>
|
||||
#include <uORB/topics/telemetry_status.h>
|
||||
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <systemlib/param/param.h>
|
||||
@@ -758,6 +759,7 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||
struct esc_status_s esc;
|
||||
struct vehicle_global_velocity_setpoint_s global_vel_sp;
|
||||
struct battery_status_s battery;
|
||||
struct telemetry_status_s telemetry;
|
||||
} buf;
|
||||
|
||||
memset(&buf, 0, sizeof(buf));
|
||||
@@ -783,6 +785,7 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||
int esc_sub;
|
||||
int global_vel_sp_sub;
|
||||
int battery_sub;
|
||||
int telemetry_sub;
|
||||
} subs;
|
||||
|
||||
/* log message buffer: header + body */
|
||||
@@ -811,6 +814,7 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||
struct log_GVSP_s log_GVSP;
|
||||
struct log_BATT_s log_BATT;
|
||||
struct log_DIST_s log_DIST;
|
||||
struct log_TELE_s log_TELE;
|
||||
} body;
|
||||
} log_msg = {
|
||||
LOG_PACKET_HEADER_INIT(0)
|
||||
@@ -946,6 +950,12 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||
fds[fdsc_count].events = POLLIN;
|
||||
fdsc_count++;
|
||||
|
||||
/* --- TELEMETRY STATUS --- */
|
||||
subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status));
|
||||
fds[fdsc_count].fd = subs.telemetry_sub;
|
||||
fds[fdsc_count].events = POLLIN;
|
||||
fdsc_count++;
|
||||
|
||||
/* WARNING: If you get the error message below,
|
||||
* then the number of registered messages (fdsc)
|
||||
* differs from the number of messages in the above list.
|
||||
@@ -1347,6 +1357,20 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||
LOGBUFFER_WRITE_AND_COUNT(BATT);
|
||||
}
|
||||
|
||||
/* --- TELEMETRY --- */
|
||||
if (fds[ifds++].revents & POLLIN) {
|
||||
orb_copy(ORB_ID(telemetry_status), subs.telemetry_sub, &buf.telemetry);
|
||||
log_msg.msg_type = LOG_TELE_MSG;
|
||||
log_msg.body.log_TELE.rssi = buf.telemetry.rssi;
|
||||
log_msg.body.log_TELE.remote_rssi = buf.telemetry.remote_rssi;
|
||||
log_msg.body.log_TELE.noise = buf.telemetry.noise;
|
||||
log_msg.body.log_TELE.remote_noise = buf.telemetry.remote_noise;
|
||||
log_msg.body.log_TELE.rxerrors = buf.telemetry.rxerrors;
|
||||
log_msg.body.log_TELE.fixed = buf.telemetry.fixed;
|
||||
log_msg.body.log_TELE.txbuf = buf.telemetry.txbuf;
|
||||
LOGBUFFER_WRITE_AND_COUNT(TELE);
|
||||
}
|
||||
|
||||
/* signal the other thread new data, but not yet unlock */
|
||||
if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) {
|
||||
/* only request write if several packets can be written at once */
|
||||
|
||||
@@ -264,6 +264,18 @@ struct log_DIST_s {
|
||||
uint8_t flags;
|
||||
};
|
||||
|
||||
/* --- TELE - TELEMETRY STATUS --- */
|
||||
#define LOG_TELE_MSG 22
|
||||
struct log_TELE_s {
|
||||
uint8_t rssi;
|
||||
uint8_t remote_rssi;
|
||||
uint8_t noise;
|
||||
uint8_t remote_noise;
|
||||
uint16_t rxerrors;
|
||||
uint16_t fixed;
|
||||
uint8_t txbuf;
|
||||
};
|
||||
|
||||
/********** SYSTEM MESSAGES, ID > 0x80 **********/
|
||||
|
||||
/* --- TIME - TIME STAMP --- */
|
||||
@@ -311,6 +323,8 @@ static const struct log_format_s log_formats[] = {
|
||||
LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
|
||||
LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"),
|
||||
LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"),
|
||||
LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"),
|
||||
|
||||
/* system-level messages, ID >= 0x80 */
|
||||
// FMT: don't write format of format message, it's useless
|
||||
LOG_FORMAT(TIME, "Q", "StartTime"),
|
||||
|
||||
@@ -58,10 +58,10 @@ enum TELEMETRY_STATUS_RADIO_TYPE {
|
||||
struct telemetry_status_s {
|
||||
uint64_t timestamp;
|
||||
enum TELEMETRY_STATUS_RADIO_TYPE type; /**< type of the radio hardware */
|
||||
unsigned rssi; /**< local signal strength */
|
||||
unsigned remote_rssi; /**< remote signal strength */
|
||||
unsigned rxerrors; /**< receive errors */
|
||||
unsigned fixed; /**< count of error corrected packets */
|
||||
uint8_t rssi; /**< local signal strength */
|
||||
uint8_t remote_rssi; /**< remote signal strength */
|
||||
uint16_t rxerrors; /**< receive errors */
|
||||
uint16_t fixed; /**< count of error corrected packets */
|
||||
uint8_t noise; /**< background noise level */
|
||||
uint8_t remote_noise; /**< remote background noise level */
|
||||
uint8_t txbuf; /**< how full the tx buffer is as a percentage */
|
||||
@@ -73,4 +73,4 @@ struct telemetry_status_s {
|
||||
|
||||
ORB_DECLARE(telemetry_status);
|
||||
|
||||
#endif /* TOPIC_TELEMETRY_STATUS_H */
|
||||
#endif /* TOPIC_TELEMETRY_STATUS_H */
|
||||
|
||||
Reference in New Issue
Block a user