forked from Archive/PX4-Autopilot
Merge branch 'master' of github.com:PX4/Firmware into indoor
This commit is contained in:
commit
ebf7e1c22f
|
@ -23,7 +23,7 @@ mavlink stream -d /dev/ttyACM0 -s RC_CHANNELS_RAW -r 5
|
|||
usleep 100000
|
||||
mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 20
|
||||
usleep 100000
|
||||
mavlink stream -d /dev/ttyACM0 -s GLOBAL_POSITION_SETPOINT_INT -r 20
|
||||
mavlink stream -d /dev/ttyACM0 -s POSITION_TARGET_GLOBAL_INT -r 10
|
||||
usleep 100000
|
||||
|
||||
# Exit shell to make it available to MAVLink
|
||||
|
|
|
@ -25,7 +25,6 @@ MODULES += drivers/mpu6000
|
|||
MODULES += drivers/hmc5883
|
||||
MODULES += drivers/ms5611
|
||||
MODULES += drivers/mb12xx
|
||||
MODULES += drivers/ll40ls
|
||||
MODULES += drivers/gps
|
||||
MODULES += drivers/hil
|
||||
MODULES += drivers/hott/hott_telemetry
|
||||
|
@ -44,7 +43,6 @@ MODULES += modules/sensors
|
|||
#
|
||||
MODULES += systemcmds/mtd
|
||||
MODULES += systemcmds/bl_update
|
||||
MODULES += systemcmds/i2c
|
||||
MODULES += systemcmds/mixer
|
||||
MODULES += systemcmds/param
|
||||
MODULES += systemcmds/perf
|
||||
|
@ -152,5 +150,4 @@ endef
|
|||
# command priority stack entrypoint
|
||||
BUILTIN_COMMANDS := \
|
||||
$(call _B, sercon, , 2048, sercon_main ) \
|
||||
$(call _B, serdis, , 2048, serdis_main ) \
|
||||
$(call _B, sysinfo, , 2048, sysinfo_main )
|
||||
$(call _B, serdis, , 2048, serdis_main )
|
||||
|
|
|
@ -165,7 +165,7 @@ Airspeed::probe()
|
|||
*/
|
||||
_retries = 4;
|
||||
int ret = measure();
|
||||
_retries = 2;
|
||||
_retries = 0;
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
@ -381,7 +381,10 @@ Airspeed::cycle_trampoline(void *arg)
|
|||
Airspeed *dev = (Airspeed *)arg;
|
||||
|
||||
dev->cycle();
|
||||
dev->update_status();
|
||||
// XXX we do not know if this is
|
||||
// really helping - do not update the
|
||||
// subsys state right now
|
||||
//dev->update_status();
|
||||
}
|
||||
|
||||
void
|
||||
|
|
|
@ -1283,14 +1283,13 @@ int HMC5883::set_excitement(unsigned enable)
|
|||
if (OK != ret)
|
||||
perf_count(_comms_errors);
|
||||
|
||||
_conf_reg &= ~0x03; // reset previous excitement mode
|
||||
if (((int)enable) < 0) {
|
||||
_conf_reg |= 0x01;
|
||||
|
||||
} else if (enable > 0) {
|
||||
_conf_reg |= 0x02;
|
||||
|
||||
} else {
|
||||
_conf_reg &= ~0x03;
|
||||
}
|
||||
|
||||
// ::printf("set_excitement enable=%d regA=0x%x\n", (int)enable, (unsigned)_conf_reg);
|
||||
|
|
|
@ -130,7 +130,7 @@ protected:
|
|||
float _T;
|
||||
|
||||
/* altitude conversion calibration */
|
||||
unsigned _msl_pressure; /* in kPa */
|
||||
unsigned _msl_pressure; /* in Pa */
|
||||
|
||||
orb_advert_t _baro_topic;
|
||||
|
||||
|
|
|
@ -1968,7 +1968,7 @@ PX4IO::print_status(bool extended_status)
|
|||
printf("actuators");
|
||||
|
||||
for (unsigned i = 0; i < _max_actuators; i++)
|
||||
printf(" %u", io_reg_get(PX4IO_PAGE_ACTUATORS, i));
|
||||
printf(" %hi", int16_t(io_reg_get(PX4IO_PAGE_ACTUATORS, i)));
|
||||
|
||||
printf("\n");
|
||||
printf("servos");
|
||||
|
|
|
@ -598,7 +598,8 @@ SF0X::collect()
|
|||
memcpy(_linebuf, buf, (lend + 1) - (i + 1));
|
||||
}
|
||||
|
||||
if (_linebuf[i] == '.') {
|
||||
/* we need a digit before the dot and a dot for a valid number */
|
||||
if (i > 0 && _linebuf[i] == '.') {
|
||||
valid = true;
|
||||
}
|
||||
}
|
||||
|
|
|
@ -169,7 +169,7 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch,
|
|||
|
||||
if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {
|
||||
|
||||
float id = _rate_error * dt;
|
||||
float id = _rate_error * dt * scaler;
|
||||
|
||||
/*
|
||||
* anti-windup: do not allow integrator to increase if actuator is at limit
|
||||
|
@ -190,7 +190,9 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch,
|
|||
float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
|
||||
|
||||
/* Apply PI rate controller and store non-limited output */
|
||||
_last_output = (_bodyrate_setpoint * _k_ff +_rate_error * _k_p + integrator_constrained) * scaler * scaler; //scaler is proportional to 1/airspeed
|
||||
_last_output = _bodyrate_setpoint * _k_ff * scaler +
|
||||
_rate_error * _k_p * scaler * scaler
|
||||
+ integrator_constrained; //scaler is proportional to 1/airspeed
|
||||
// warnx("pitch: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);
|
||||
// warnx("roll: _last_output %.4f", (double)_last_output);
|
||||
return math::constrain(_last_output, -1.0f, 1.0f);
|
||||
|
|
|
@ -135,7 +135,7 @@ float ECL_RollController::control_bodyrate(float pitch,
|
|||
|
||||
if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {
|
||||
|
||||
float id = _rate_error * dt;
|
||||
float id = _rate_error * dt * scaler;
|
||||
|
||||
/*
|
||||
* anti-windup: do not allow integrator to increase if actuator is at limit
|
||||
|
@ -157,7 +157,9 @@ float ECL_RollController::control_bodyrate(float pitch,
|
|||
//warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max);
|
||||
|
||||
/* Apply PI rate controller and store non-limited output */
|
||||
_last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * scaler * scaler; //scaler is proportional to 1/airspeed
|
||||
_last_output = _bodyrate_setpoint * _k_ff * scaler +
|
||||
_rate_error * _k_p * scaler * scaler
|
||||
+ integrator_constrained; //scaler is proportional to 1/airspeed
|
||||
|
||||
return math::constrain(_last_output, -1.0f, 1.0f);
|
||||
}
|
||||
|
|
|
@ -129,7 +129,7 @@ extern struct system_load_s system_load;
|
|||
#define POSITION_TIMEOUT (2 * 1000 * 1000) /**< consider the local or global position estimate invalid after 600ms */
|
||||
#define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */
|
||||
#define RC_TIMEOUT 500000
|
||||
#define DL_TIMEOUT 5 * 1000* 1000
|
||||
#define DL_TIMEOUT (10 * 1000 * 1000)
|
||||
#define OFFBOARD_TIMEOUT 500000
|
||||
#define DIFFPRESS_TIMEOUT 2000000
|
||||
|
||||
|
|
|
@ -217,6 +217,8 @@ Mavlink::Mavlink() :
|
|||
errx(1, "instance ID is out of range");
|
||||
break;
|
||||
}
|
||||
|
||||
_rstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
|
||||
}
|
||||
|
||||
Mavlink::~Mavlink()
|
||||
|
@ -1388,8 +1390,8 @@ Mavlink::task_main(int argc, char *argv[])
|
|||
configure_stream("GLOBAL_POSITION_INT", 3.0f);
|
||||
configure_stream("LOCAL_POSITION_NED", 3.0f);
|
||||
configure_stream("RC_CHANNELS_RAW", 1.0f);
|
||||
configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f);
|
||||
configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f);
|
||||
configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f);
|
||||
configure_stream("ATTITUDE_TARGET", 3.0f);
|
||||
configure_stream("DISTANCE_SENSOR", 0.5f);
|
||||
break;
|
||||
|
||||
|
@ -1653,6 +1655,8 @@ Mavlink::display_status()
|
|||
printf("\tGCS heartbeat:\t%llu us ago\n", hrt_elapsed_time(&_rstatus.heartbeat_time));
|
||||
}
|
||||
|
||||
printf("\tmavlink chan: #%u\n", _channel);
|
||||
|
||||
if (_rstatus.timestamp > 0) {
|
||||
|
||||
printf("\ttype:\t\t");
|
||||
|
|
|
@ -1333,7 +1333,7 @@ protected:
|
|||
}
|
||||
|
||||
for (unsigned i = 0; i < 8; i++) {
|
||||
if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
|
||||
if (act.output[i] > PWM_LOWEST_MIN / 2) {
|
||||
if (i < n) {
|
||||
/* scale PWM out 900..2100 us to 0..1 for rotors */
|
||||
out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
|
||||
|
@ -1344,7 +1344,7 @@ protected:
|
|||
}
|
||||
|
||||
} else {
|
||||
/* send 0 when disarmed */
|
||||
/* send 0 when disarmed and for disabled channels */
|
||||
out[i] = 0.0f;
|
||||
}
|
||||
}
|
||||
|
@ -1353,6 +1353,7 @@ protected:
|
|||
/* fixed wing: scale throttle to 0..1 and other channels to -1..1 */
|
||||
|
||||
for (unsigned i = 0; i < 8; i++) {
|
||||
if (act.output[i] > PWM_LOWEST_MIN / 2) {
|
||||
if (i != 3) {
|
||||
/* scale PWM out 900..2100 us to -1..1 for normal channels */
|
||||
out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2);
|
||||
|
@ -1362,6 +1363,10 @@ protected:
|
|||
out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
|
||||
}
|
||||
|
||||
} else {
|
||||
/* set 0 for disabled channels */
|
||||
out[i] = 0.0f;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -1637,10 +1642,10 @@ protected:
|
|||
msg.chan2_raw = (rc.channel_count > (i * port_width) + 1) ? rc.values[(i * port_width) + 1] : UINT16_MAX;
|
||||
msg.chan3_raw = (rc.channel_count > (i * port_width) + 2) ? rc.values[(i * port_width) + 2] : UINT16_MAX;
|
||||
msg.chan4_raw = (rc.channel_count > (i * port_width) + 3) ? rc.values[(i * port_width) + 3] : UINT16_MAX;
|
||||
msg.chan4_raw = (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX;
|
||||
msg.chan4_raw = (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX;
|
||||
msg.chan4_raw = (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX;
|
||||
msg.chan4_raw = (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX;
|
||||
msg.chan5_raw = (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX;
|
||||
msg.chan6_raw = (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX;
|
||||
msg.chan7_raw = (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX;
|
||||
msg.chan8_raw = (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX;
|
||||
msg.rssi = rc.rssi;
|
||||
|
||||
_mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS_RAW, &msg);
|
||||
|
|
|
@ -112,7 +112,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
|
|||
_telemetry_status_pub(-1),
|
||||
_rc_pub(-1),
|
||||
_manual_pub(-1),
|
||||
_radio_status_available(false),
|
||||
_control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))),
|
||||
_hil_frames(0),
|
||||
_old_timestamp(0),
|
||||
|
@ -137,6 +136,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
|
|||
handle_message_command_long(msg);
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_COMMAND_INT:
|
||||
handle_message_command_int(msg);
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_OPTICAL_FLOW:
|
||||
handle_message_optical_flow(msg);
|
||||
break;
|
||||
|
@ -276,6 +279,62 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
|
|||
}
|
||||
}
|
||||
|
||||
void
|
||||
MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
|
||||
{
|
||||
/* command */
|
||||
mavlink_command_int_t cmd_mavlink;
|
||||
mavlink_msg_command_int_decode(msg, &cmd_mavlink);
|
||||
|
||||
if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid)
|
||||
|| (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) {
|
||||
//check for MAVLINK terminate command
|
||||
if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) {
|
||||
/* This is the link shutdown command, terminate mavlink */
|
||||
warnx("terminated by remote command");
|
||||
fflush(stdout);
|
||||
usleep(50000);
|
||||
|
||||
/* terminate other threads and this thread */
|
||||
_mavlink->_task_should_exit = true;
|
||||
|
||||
} else {
|
||||
|
||||
if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) {
|
||||
warnx("ignoring CMD spoofed with same SYS/COMP (%d/%d) ID",
|
||||
mavlink_system.sysid, mavlink_system.compid);
|
||||
return;
|
||||
}
|
||||
|
||||
struct vehicle_command_s vcmd;
|
||||
memset(&vcmd, 0, sizeof(vcmd));
|
||||
|
||||
/* Copy the content of mavlink_command_int_t cmd_mavlink into command_t cmd */
|
||||
vcmd.param1 = cmd_mavlink.param1;
|
||||
vcmd.param2 = cmd_mavlink.param2;
|
||||
vcmd.param3 = cmd_mavlink.param3;
|
||||
vcmd.param4 = cmd_mavlink.param4;
|
||||
/* these are coordinates as 1e7 scaled integers to work around the 32 bit floating point limits */
|
||||
vcmd.param5 = ((double)cmd_mavlink.x) / 1e7;
|
||||
vcmd.param6 = ((double)cmd_mavlink.y) / 1e7;
|
||||
vcmd.param7 = cmd_mavlink.z;
|
||||
// XXX do proper translation
|
||||
vcmd.command = (enum VEHICLE_CMD)cmd_mavlink.command;
|
||||
vcmd.target_system = cmd_mavlink.target_system;
|
||||
vcmd.target_component = cmd_mavlink.target_component;
|
||||
vcmd.source_system = msg->sysid;
|
||||
vcmd.source_component = msg->compid;
|
||||
|
||||
if (_cmd_pub < 0) {
|
||||
_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg)
|
||||
{
|
||||
|
@ -430,9 +489,6 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
|
|||
} else {
|
||||
orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus);
|
||||
}
|
||||
|
||||
/* this means that heartbeats alone won't be published to the radio status no more */
|
||||
_radio_status_available = true;
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -474,18 +530,11 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
|
|||
|
||||
struct telemetry_status_s &tstatus = _mavlink->get_rx_status();
|
||||
|
||||
hrt_abstime tnow = hrt_absolute_time();
|
||||
|
||||
/* always set heartbeat, publish only if telemetry link not up */
|
||||
tstatus.heartbeat_time = tnow;
|
||||
|
||||
/* if no radio status messages arrive, lets at least publish that heartbeats were received */
|
||||
if (!_radio_status_available) {
|
||||
|
||||
tstatus.timestamp = tnow;
|
||||
/* telem_time indicates the timestamp of a telemetry status packet and we got none */
|
||||
tstatus.telem_time = 0;
|
||||
tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
|
||||
/* set heartbeat time and topic time and publish -
|
||||
* the telem status also gets updated on telemetry events
|
||||
*/
|
||||
tstatus.timestamp = hrt_absolute_time();
|
||||
tstatus.heartbeat_time = tstatus.timestamp;
|
||||
|
||||
if (_telemetry_status_pub < 0) {
|
||||
_telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus);
|
||||
|
@ -496,7 +545,6 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
|
|||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
MavlinkReceiver::handle_message_request_data_stream(mavlink_message_t *msg)
|
||||
|
|
|
@ -110,6 +110,7 @@ private:
|
|||
|
||||
void handle_message(mavlink_message_t *msg);
|
||||
void handle_message_command_long(mavlink_message_t *msg);
|
||||
void handle_message_command_int(mavlink_message_t *msg);
|
||||
void handle_message_optical_flow(mavlink_message_t *msg);
|
||||
void handle_message_set_mode(mavlink_message_t *msg);
|
||||
void handle_message_vicon_position_estimate(mavlink_message_t *msg);
|
||||
|
@ -151,7 +152,6 @@ private:
|
|||
orb_advert_t _telemetry_status_pub;
|
||||
orb_advert_t _rc_pub;
|
||||
orb_advert_t _manual_pub;
|
||||
bool _radio_status_available;
|
||||
int _control_mode_sub;
|
||||
int _hil_frames;
|
||||
uint64_t _old_timestamp;
|
||||
|
|
|
@ -214,6 +214,16 @@ PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f);
|
|||
*/
|
||||
PARAM_DEFINE_FLOAT(SENS_DPRES_ANSC, 0);
|
||||
|
||||
/**
|
||||
* QNH for barometer
|
||||
*
|
||||
* @min 500
|
||||
* @max 1500
|
||||
* @group Sensor Calibration
|
||||
* @unit hPa
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SENS_BARO_QNH, 1013.25f);
|
||||
|
||||
|
||||
/**
|
||||
* Board rotation
|
||||
|
|
|
@ -143,6 +143,12 @@
|
|||
|
||||
#define STICK_ON_OFF_LIMIT 0.75f
|
||||
|
||||
/* oddly, ERROR is not defined for c++ */
|
||||
#ifdef ERROR
|
||||
# undef ERROR
|
||||
#endif
|
||||
static const int ERROR = -1;
|
||||
|
||||
/**
|
||||
* Sensor app start / stop handling function
|
||||
*
|
||||
|
@ -301,6 +307,8 @@ private:
|
|||
float battery_voltage_scaling;
|
||||
float battery_current_scaling;
|
||||
|
||||
float baro_qnh;
|
||||
|
||||
} _parameters; /**< local copies of interesting parameters */
|
||||
|
||||
struct {
|
||||
|
@ -357,6 +365,8 @@ private:
|
|||
|
||||
param_t board_offset[3];
|
||||
|
||||
param_t baro_qnh;
|
||||
|
||||
} _parameter_handles; /**< handles for interesting parameters */
|
||||
|
||||
|
||||
|
@ -462,12 +472,6 @@ private:
|
|||
namespace sensors
|
||||
{
|
||||
|
||||
/* oddly, ERROR is not defined for c++ */
|
||||
#ifdef ERROR
|
||||
# undef ERROR
|
||||
#endif
|
||||
static const int ERROR = -1;
|
||||
|
||||
Sensors *g_sensors = nullptr;
|
||||
}
|
||||
|
||||
|
@ -617,6 +621,9 @@ Sensors::Sensors() :
|
|||
_parameter_handles.board_offset[1] = param_find("SENS_BOARD_Y_OFF");
|
||||
_parameter_handles.board_offset[2] = param_find("SENS_BOARD_Z_OFF");
|
||||
|
||||
/* Barometer QNH */
|
||||
_parameter_handles.baro_qnh = param_find("SENS_BARO_QNH");
|
||||
|
||||
/* fetch initial parameter values */
|
||||
parameters_update();
|
||||
}
|
||||
|
@ -841,6 +848,25 @@ Sensors::parameters_update()
|
|||
|
||||
_board_rotation = _board_rotation * board_rotation_offset;
|
||||
|
||||
/* update barometer qnh setting */
|
||||
param_get(_parameter_handles.baro_qnh, &(_parameters.baro_qnh));
|
||||
int fd;
|
||||
fd = open(BARO_DEVICE_PATH, 0);
|
||||
if (fd < 0) {
|
||||
warn("%s", BARO_DEVICE_PATH);
|
||||
errx(1, "FATAL: no barometer found");
|
||||
|
||||
} else {
|
||||
warnx("qnh ioctl, %lu", (unsigned long)(_parameters.baro_qnh * 100));
|
||||
int ret = ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100));
|
||||
if (ret) {
|
||||
warnx("qnh could not be set");
|
||||
close(fd);
|
||||
return ERROR;
|
||||
}
|
||||
close(fd);
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
|
|
|
@ -331,7 +331,7 @@ mag(int argc, char *argv[])
|
|||
|
||||
float len = sqrtf(buf.x * buf.x + buf.y * buf.y + buf.z * buf.z);
|
||||
|
||||
if (len < 1.0f || len > 3.0f) {
|
||||
if (len < 0.25f || len > 3.0f) {
|
||||
warnx("MAG scale error!");
|
||||
return ERROR;
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue