void Mavlink::mavlink_update_system(void)
{
if (!_param_initialized) {
_param_system_id = param_find("MAV_SYS_ID");
_param_component_id = param_find("MAV_COMP_ID");
_param_system_type = param_find("MAV_TYPE");
_param_use_hil_gps = param_find("MAV_USEHILGPS");
}
/* update system and component id */
int32_t system_id;
param_get(_param_system_id, &system_id);
int32_t component_id;
param_get(_param_component_id, &component_id);
/* only allow system ID and component ID updates
* after reboot - not during operation */
if (!_param_initialized) {
if (system_id > 0 && system_id < 255) {
mavlink_system.sysid = system_id;
}
if (component_id > 0 && component_id < 255) {
mavlink_system.compid = component_id;
}
_param_initialized = true;
}
/* warn users that they need to reboot to take this
* into effect
*/
if (system_id != mavlink_system.sysid) {
send_statustext_critical("Save params and reboot to change SYSID");
}
if (component_id != mavlink_system.compid) {
send_statustext_critical("Save params and reboot to change COMPID");
}
int32_t system_type;
param_get(_param_system_type, &system_type);
if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) {
mavlink_system.type = system_type;
}
int32_t use_hil_gps;
param_get(_param_use_hil_gps, &use_hil_gps);
_use_hil_gps = (bool)use_hil_gps;
}
static bool imuConsistencyCheck(orb_advert_t *mavlink_log_pub, bool report_status)
{
bool success = true; // start with a pass and change to a fail if any test fails
float test_limit = 1.0f; // pass limit re-used for each test
// Get sensor_preflight data if available and exit with a fail recorded if not
int sensors_sub = orb_subscribe(ORB_ID(sensor_preflight));
sensor_preflight_s sensors = {};
if (orb_copy(ORB_ID(sensor_preflight), sensors_sub, &sensors) != PX4_OK) {
goto out;
}
// Use the difference between IMU's to detect a bad calibration.
// If a single IMU is fitted, the value being checked will be zero so this check will always pass.
param_get(param_find("COM_ARM_IMU_ACC"), &test_limit);
if (sensors.accel_inconsistency_m_s_s > test_limit) {
if (report_status) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCELS INCONSISTENT - CHECK CAL");
}
success = false;
goto out;
} else if (sensors.accel_inconsistency_m_s_s > test_limit * 0.8f) {
if (report_status) {
mavlink_log_info(mavlink_log_pub, "PREFLIGHT ADVICE: ACCELS INCONSISTENT - CHECK CAL");
}
}
// Fail if gyro difference greater than 5 deg/sec and notify if greater than 2.5 deg/sec
param_get(param_find("COM_ARM_IMU_GYR"), &test_limit);
if (sensors.gyro_inconsistency_rad_s > test_limit) {
if (report_status) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GYROS INCONSISTENT - CHECK CAL");
}
success = false;
goto out;
} else if (sensors.gyro_inconsistency_rad_s > test_limit * 0.5f) {
if (report_status) {
mavlink_log_info(mavlink_log_pub, "PREFLIGHT ADVICE: GYROS INCONSISTENT - CHECK CAL");
}
}
out:
orb_unsubscribe(sensors_sub);
return success;
}
void
Syslink::update_params(bool force_set)
{
param_t _param_radio_channel = param_find("SLNK_RADIO_CHAN");
param_t _param_radio_rate = param_find("SLNK_RADIO_RATE");
param_t _param_radio_addr1 = param_find("SLNK_RADIO_ADDR1");
param_t _param_radio_addr2 = param_find("SLNK_RADIO_ADDR2");
// reading parameter values into temp variables
int32_t channel, rate, addr1, addr2;
uint64_t addr = 0;
param_get(_param_radio_channel, &channel);
param_get(_param_radio_rate, &rate);
param_get(_param_radio_addr1, &addr1);
param_get(_param_radio_addr2, &addr2);
memcpy(&addr, &addr2, 4);
memcpy(((char *)&addr) + 4, &addr1, 4);
hrt_abstime t = hrt_absolute_time();
// request updates if needed
if (channel != this->_channel || force_set) {
this->_channel = channel;
set_channel(channel);
this->_params_update[0] = t;
this->_params_ack[0] = 0;
}
if (rate != this->_rate || force_set) {
this->_rate = rate;
set_datarate(rate);
this->_params_update[1] = t;
this->_params_ack[1] = 0;
}
if (addr != this->_addr || force_set) {
this->_addr = addr;
set_address(addr);
this->_params_update[2] = t;
this->_params_ack[2] = 0;
}
}
InputMavlinkCmdMount::InputMavlinkCmdMount(bool stabilize)
: _stabilize {stabilize, stabilize, stabilize}
{
param_t handle = param_find("MAV_SYS_ID");
if (handle != PARAM_INVALID) {
param_get(handle, &_mav_sys_id);
}
handle = param_find("MAV_COMP_ID");
if (handle != PARAM_INVALID) {
param_get(handle, &_mav_comp_id);
}
}
float battery_remaining_estimate_voltage(float voltage, float discharged)
{
float ret = 0;
static param_t bat_v_empty_h;
static param_t bat_v_full_h;
static param_t bat_n_cells_h;
static param_t bat_capacity_h;
static float bat_v_empty = 3.2f;
static float bat_v_full = 4.0f;
static int bat_n_cells = 3;
static float bat_capacity = -1.0f;
static bool initialized = false;
static unsigned int counter = 0;
if (!initialized) {
bat_v_empty_h = param_find("BAT_V_EMPTY");
bat_v_full_h = param_find("BAT_V_FULL");
bat_n_cells_h = param_find("BAT_N_CELLS");
bat_capacity_h = param_find("BAT_CAPACITY");
initialized = true;
}
if (counter % 100 == 0) {
param_get(bat_v_empty_h, &bat_v_empty);
param_get(bat_v_full_h, &bat_v_full);
param_get(bat_n_cells_h, &bat_n_cells);
param_get(bat_capacity_h, &bat_capacity);
}
counter++;
/* remaining charge estimate based on voltage */
float remaining_voltage = (voltage - bat_n_cells * bat_v_empty) / (bat_n_cells * (bat_v_full - bat_v_empty));
if (bat_capacity > 0.0f) {
/* if battery capacity is known, use discharged current for estimate, but don't show more than voltage estimate */
ret = fminf(remaining_voltage, 1.0f - discharged / bat_capacity);
} else {
/* else use voltage */
ret = remaining_voltage;
}
/* limit to sane values */
ret = (ret < 0.0f) ? 0.0f : ret;
ret = (ret > 1.0f) ? 1.0f : ret;
return ret;
}
int TemperatureCalibrationBaro::finish_sensor_instance(PerSensorData &data, int sensor_index)
{
if (!data.hot_soaked || data.tempcal_complete) {
return 0;
}
double res[POLYFIT_ORDER + 1] = {};
data.P[0].fit(res);
res[POLYFIT_ORDER] =
0.0; // normalise the correction to be zero at the reference temperature by setting the X^0 coefficient to zero
PX4_INFO("Result baro %u %.20f %.20f %.20f %.20f %.20f %.20f", sensor_index, (double)res[0],
(double)res[1], (double)res[2], (double)res[3], (double)res[4], (double)res[5]);
data.tempcal_complete = true;
char str[30];
float param = 0.0f;
int result = PX4_OK;
set_parameter("TC_B%d_ID", sensor_index, &data.device_id);
for (unsigned coef_index = 0; coef_index <= POLYFIT_ORDER; coef_index++) {
sprintf(str, "TC_B%d_X%d", sensor_index, (POLYFIT_ORDER - coef_index));
param = (float)res[coef_index];
result = param_set_no_notification(param_find(str), ¶m);
if (result != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
}
set_parameter("TC_B%d_TMAX", sensor_index, &data.high_temp);
set_parameter("TC_B%d_TMIN", sensor_index, &data.low_temp);
set_parameter("TC_B%d_TREF", sensor_index, &data.ref_temp);
return 0;
}
void CameraInterface::get_pins()
{
// Get parameter handle
_p_pin = param_find("TRIG_PINS");
int pin_list;
param_get(_p_pin, &pin_list);
// Set all pins as invalid
for (unsigned i = 0; i < arraySize(_pins); i++) {
_pins[i] = -1;
}
// Convert number to individual channels
unsigned i = 0;
int single_pin;
while ((single_pin = pin_list % 10)) {
_pins[i] = single_pin - 1;
if (_pins[i] < 0) {
_pins[i] = -1;
}
pin_list /= 10;
i++;
}
}
BlockParamBase::BlockParamBase(Block *parent, const char *name, bool parent_prefix) :
_handle(PARAM_INVALID)
{
char fullname[blockNameLengthMax];
if (parent == NULL) {
strncpy(fullname, name, blockNameLengthMax);
} else {
char parentName[blockNameLengthMax];
parent->getName(parentName, blockNameLengthMax);
if (!strcmp(name, "")) {
strncpy(fullname, parentName, blockNameLengthMax);
// ensure string is terminated
fullname[sizeof(fullname) - 1] = '\0';
} else if (parent_prefix) {
snprintf(fullname, blockNameLengthMax, "%s_%s", parentName, name);
} else {
strncpy(fullname, name, blockNameLengthMax);
// ensure string is terminated
fullname[sizeof(fullname) - 1] = '\0';
}
parent->getParams().add(this);
}
_handle = param_find(fullname);
if (_handle == PARAM_INVALID) {
printf("error finding param: %s\n", fullname);
}
};
bool circuit_breaker_enabled(const char* breaker, int32_t magic)
{
int32_t val;
(void)param_get(param_find(breaker), &val);
return (val == magic);
}
Standard::Standard(VtolAttitudeControl *attc) :
VtolType(attc),
_flag_enable_mc_motors(true),
_pusher_throttle(0.0f),
_mc_att_ctl_weight(1.0f),
_airspeed_trans_blend_margin(0.0f)
{
_vtol_schedule.flight_mode = MC_MODE;
_vtol_schedule.transition_start = 0;
_params_handles_standard.front_trans_dur = param_find("VT_F_TRANS_DUR");
_params_handles_standard.back_trans_dur = param_find("VT_B_TRANS_DUR");
_params_handles_standard.pusher_trans = param_find("VT_TRANS_THR");
_params_handles_standard.airspeed_blend = param_find("VT_ARSP_BLEND");
_params_handles_standard.airspeed_trans = param_find("VT_ARSP_TRANS");
}
int
test_param(int argc, char *argv[])
{
param_t p;
p = param_find("test");
if (p == PARAM_INVALID)
errx(1, "test parameter not found");
param_type_t t = param_type(p);
if (t != PARAM_TYPE_INT32)
errx(1, "test parameter type mismatch (got %u)", (unsigned)t);
int32_t val;
if (param_get(p, &val) != 0)
errx(1, "failed to read test parameter");
if (val != 0x12345678)
errx(1, "parameter value mismatch (val = %X)", val);
val = 0xa5a5a5a5;
if (param_set(p, &val) != 0)
errx(1, "failed to write test parameter");
if (param_get(p, &val) != 0)
errx(1, "failed to re-read test parameter");
if ((uint32_t)val != 0xa5a5a5a5)
errx(1, "parameter value mismatch after write (val = %X)", val);
warnx("parameter test PASS");
return 0;
}
void GPS::initializeCommunicationDump()
{
param_t gps_dump_comm_ph = param_find("GPS_DUMP_COMM");
int32_t param_dump_comm;
if (gps_dump_comm_ph == PARAM_INVALID || param_get(gps_dump_comm_ph, ¶m_dump_comm) != 0) {
return;
}
if (param_dump_comm != 1) {
return; //dumping disabled
}
_dump_from_device = new gps_dump_s();
_dump_to_device = new gps_dump_s();
if (!_dump_from_device || !_dump_to_device) {
PX4_ERR("failed to allocated dump data");
return;
}
memset(_dump_to_device, 0, sizeof(gps_dump_s));
memset(_dump_from_device, 0, sizeof(gps_dump_s));
int instance;
//make sure to use a large enough queue size, so that we don't lose messages. You may also want
//to increase the logger rate for that.
_dump_communication_pub = orb_advertise_multi_queue(ORB_ID(gps_dump), _dump_from_device, &instance,
ORB_PRIO_DEFAULT, 8);
}
CameraInterfacePWM::CameraInterfacePWM():
CameraInterface(),
_camera_is_on(false)
{
_p_pin = param_find("TRIG_PINS");
int pin_list;
param_get(_p_pin, &pin_list);
// Set all pins as invalid
for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
_pins[i] = -1;
}
// Convert number to individual channels
unsigned i = 0;
int single_pin;
while ((single_pin = pin_list % 10)) {
_pins[i] = single_pin - 1;
if (_pins[i] < 0) {
_pins[i] = -1;
}
pin_list /= 10;
i++;
}
setup();
}
int parameters_init(struct param_handles *h)
{
/* PID parameters */
h->yaw_p = param_find("RV_YAW_P");
return OK;
}
// return false if the magnetomer measurements are inconsistent
static bool magConsistencyCheck(orb_advert_t *mavlink_log_pub, bool report_status)
{
// get the sensor preflight data
int sensors_sub = orb_subscribe(ORB_ID(sensor_preflight));
struct sensor_preflight_s sensors = {};
if (orb_copy(ORB_ID(sensor_preflight), sensors_sub, &sensors) != 0) {
// can happen if not advertised (yet)
return true;
}
orb_unsubscribe(sensors_sub);
// Use the difference between sensors to detect a bad calibration, orientation or magnetic interference.
// If a single sensor is fitted, the value being checked will be zero so this check will always pass.
float test_limit;
param_get(param_find("COM_ARM_MAG"), &test_limit);
if (sensors.mag_inconsistency_ga > test_limit) {
if (report_status) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: MAG SENSORS INCONSISTENT");
}
return false;
}
return true;
}
FixedwingLandDetector::FixedwingLandDetector() : LandDetector(),
_paramHandle(),
_params(),
_vehicleLocalPositionSub(-1),
_vehicleLocalPosition({}),
_airspeedSub(-1),
_airspeed({}),
_parameterSub(-1),
_velocity_xy_filtered(0.0f),
_velocity_z_filtered(0.0f),
_airspeed_filtered(0.0f),
_landDetectTrigger(0)
{
_paramHandle.maxVelocity = param_find("LNDFW_VEL_XY_MAX");
_paramHandle.maxClimbRate = param_find("LNDFW_VEL_Z_MAX");
_paramHandle.maxAirSpeed = param_find("LNDFW_AIRSPD_MAX");
}
int uavcannode_start(int argc, char *argv[])
{
resources("Before board_app_initialize");
board_app_initialize(NULL);
resources("After board_app_initialize");
// CAN bitrate
int32_t bitrate = 0;
// Node ID
int32_t node_id = 0;
// Did the bootloader auto baud and get a node ID Allocated
bootloader_app_shared_t shared;
int valid = bootloader_app_shared_read(&shared, BootLoader);
if (valid == 0) {
bitrate = shared.bus_speed;
node_id = shared.node_id;
// Invalidate to prevent deja vu
bootloader_app_shared_invalidate();
} else {
// Node ID
(void)param_get(param_find("CANNODE_NODE_ID"), &node_id);
(void)param_get(param_find("CANNODE_BITRATE"), &bitrate);
}
if (node_id < 0 || node_id > uavcan::NodeID::Max || !uavcan::NodeID(node_id).isUnicast()) {
warnx("Invalid Node ID %i", node_id);
::exit(1);
}
// Start
warnx("Node ID %u, bitrate %u", node_id, bitrate);
int rv = UavcanNode::start(node_id, bitrate);
resources("After UavcanNode::start");
::sleep(1);
return rv;
}
estimator_base::estimator_base()
{
_time_to_run = -1;
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
// _airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
_gps_init = false;
_baro_init = false;
fds[0].fd = _sensor_combined_sub;
fds[0].events = POLLIN;
poll_error_counter = 0;
memset(&_params, 0, sizeof(_params));
memset(&_sensor_combined, 0, sizeof(_sensor_combined));
memset(&_gps, 0, sizeof(_gps));
memset(&_vehicle_state, 0, sizeof(_vehicle_state));
_params_handles.gravity = param_find("UAVBOOK_GRAVITY");
_params_handles.rho = param_find("UAVBOOK_RHO");
_params_handles.sigma_accel = param_find("UAVBOOK_SIGMA_ACCEL");
_params_handles.sigma_n_gps = param_find("UAVBOOK_SIGMA_N_GPS");
_params_handles.sigma_e_gps = param_find("UAVBOOK_SIGMA_E_GPS");
_params_handles.sigma_Vg_gps = param_find("UAVBOOK_SIGMA_VG_GPS");
_params_handles.sigma_course_gps = param_find("UAVBOOK_SIGMA_COURSE_GPS");
parameters_update();
_vehicle_state_pub = orb_advertise(ORB_ID(vehicle_state), &_vehicle_state);
}
MulticopterLandDetector::MulticopterLandDetector() : LandDetector(),
_paramHandle(),
_params(),
_vehicleLocalPositionSub(-1),
_actuatorsSub(-1),
_armingSub(-1),
_parameterSub(-1),
_attitudeSub(-1),
_manualSub(-1),
_vehicleLocalPosition{},
_actuators{},
_arming{},
_vehicleAttitude{},
_ctrl_state{},
_landTimer(0),
_freefallTimer(0)
{
_paramHandle.maxRotation = param_find("LNDMC_ROT_MAX");
_paramHandle.maxVelocity = param_find("LNDMC_XY_VEL_MAX");
_paramHandle.maxClimbRate = param_find("LNDMC_Z_VEL_MAX");
_paramHandle.maxThrottle = param_find("MPC_THR_MIN");
_paramHandle.minManThrottle = param_find("MPC_MANTHR_MIN");
_paramHandle.acc_threshold_m_s2 = param_find("LNDMC_FFALL_THR");
_paramHandle.ff_trigger_time = param_find("LNDMC_FFALL_TTRI");
}
LandingTargetEstimator::LandingTargetEstimator() :
_targetPosePub(nullptr),
_targetInnovationsPub(nullptr),
_paramHandle(),
_vehicleLocalPosition_valid(false),
_vehicleAttitude_valid(false),
_sensorBias_valid(false),
_new_irlockReport(false),
_estimator_initialized(false),
_faulty(false),
_last_predict(0),
_last_update(0)
{
_paramHandle.acc_unc = param_find("LTEST_ACC_UNC");
_paramHandle.meas_unc = param_find("LTEST_MEAS_UNC");
_paramHandle.pos_unc_init = param_find("LTEST_POS_UNC_IN");
_paramHandle.vel_unc_init = param_find("LTEST_VEL_UNC_IN");
_paramHandle.mode = param_find("LTEST_MODE");
_paramHandle.scale_x = param_find("LTEST_SCALE_X");
_paramHandle.scale_y = param_find("LTEST_SCALE_Y");
// Initialize uORB topics.
_initialize_topics();
_check_params(true);
}
MulticopterLandDetector::MulticopterLandDetector() : LandDetector(),
_paramHandle(),
_params(),
_vehicleLocalPositionSub(-1),
_actuatorsSub(-1),
_armingSub(-1),
_attitudeSub(-1),
_manualSub(-1),
_ctrl_state_sub(-1),
_vehicle_control_mode_sub(-1),
_vehicleLocalPosition{},
_actuators{},
_arming{},
_vehicleAttitude{},
_manual{},
_ctrl_state{},
_ctrl_mode{},
_min_trust_start(0),
_arming_time(0)
{
_paramHandle.maxRotation = param_find("LNDMC_ROT_MAX");
_paramHandle.maxVelocity = param_find("LNDMC_XY_VEL_MAX");
_paramHandle.maxClimbRate = param_find("LNDMC_Z_VEL_MAX");
_paramHandle.minThrottle = param_find("MPC_THR_MIN");
_paramHandle.minManThrottle = param_find("MPC_MANTHR_MIN");
_paramHandle.freefall_acc_threshold = param_find("LNDMC_FFALL_THR");
_paramHandle.freefall_trigger_time = param_find("LNDMC_FFALL_TTRI");
}
int UavcanNode::init(uavcan::NodeID node_id)
{
int ret = -1;
// Do regular cdev init
ret = CDev::init();
if (ret != OK) {
return ret;
}
_node.setName("org.pixhawk.pixhawk");
_node.setNodeID(node_id);
fill_node_info();
// Actuators
ret = _esc_controller.init();
if (ret < 0) {
return ret;
}
{
std::int32_t idle_throttle_when_armed = 0;
(void) param_get(param_find("UAVCAN_ESC_IDLT"), &idle_throttle_when_armed);
_esc_controller.enable_idle_throttle_when_armed(idle_throttle_when_armed > 0);
}
ret = _hardpoint_controller.init();
if (ret < 0) {
return ret;
}
// Sensor bridges
IUavcanSensorBridge::make_all(_node, _sensor_bridges);
auto br = _sensor_bridges.getHead();
while (br != nullptr) {
ret = br->init();
if (ret < 0) {
warnx("cannot init sensor bridge '%s' (%d)", br->get_name(), ret);
return ret;
}
warnx("sensor bridge '%s' init ok", br->get_name());
br = br->getSibling();
}
/* Start the Node */
return _node.start();
}
MulticopterLandDetector::MulticopterLandDetector()
{
_paramHandle.maxRotation = param_find("LNDMC_ROT_MAX");
_paramHandle.maxVelocity = param_find("LNDMC_XY_VEL_MAX");
_paramHandle.maxClimbRate = param_find("LNDMC_Z_VEL_MAX");
_paramHandle.minThrottle = param_find("MPC_THR_MIN");
_paramHandle.hoverThrottle = param_find("MPC_THR_HOVER");
_paramHandle.minManThrottle = param_find("MPC_MANTHR_MIN");
_paramHandle.freefall_acc_threshold = param_find("LNDMC_FFALL_THR");
_paramHandle.freefall_trigger_time = param_find("LNDMC_FFALL_TTRI");
_paramHandle.altitude_max = param_find("LNDMC_ALT_MAX");
_paramHandle.landSpeed = param_find("MPC_LAND_SPEED");
// Use Trigger time when transitioning from in-air (false) to landed (true) / ground contact (true).
_landed_hysteresis.set_hysteresis_time_from(false, LAND_DETECTOR_TRIGGER_TIME_US);
_maybe_landed_hysteresis.set_hysteresis_time_from(false, MAYBE_LAND_DETECTOR_TRIGGER_TIME_US);
_ground_contact_hysteresis.set_hysteresis_time_from(false, GROUND_CONTACT_TRIGGER_TIME_US);
}
static void
do_set(const char* name, const char* val)
{
int32_t i;
float f;
param_t param = param_find(name);
/* set nothing if parameter cannot be found */
if (param == PARAM_INVALID) {
/* param not found */
errx(1, "Error: Parameter %s not found.", name);
}
printf("%c %s: ",
param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'),
param_name(param));
/*
* Set parameter if type is known and conversion from string to value turns out fine
*/
switch (param_type(param)) {
case PARAM_TYPE_INT32:
if (!param_get(param, &i)) {
printf("curr: %d", i);
/* convert string */
char* end;
i = strtol(val,&end,10);
param_set(param, &i);
printf(" -> new: %d\n", i);
}
break;
case PARAM_TYPE_FLOAT:
if (!param_get(param, &f)) {
printf("curr: %4.4f", (double)f);
/* convert string */
char* end;
f = strtod(val,&end);
param_set(param, &f);
printf(" -> new: %4.4f\n", (double)f);
}
break;
default:
errx(1, "<unknown / unsupported type %d>\n", 0 + param_type(param));
}
exit(0);
}
CameraInterfaceGPIO::CameraInterfaceGPIO():
CameraInterface(),
_polarity(0)
{
_p_polarity = param_find("TRIG_POLARITY");
param_get(_p_polarity, &_polarity);
get_pins();
setup();
}
FixedwingLandDetector::FixedwingLandDetector() :
_paramHandle(),
_params(),
_controlStateSub(-1),
_armingSub(-1),
_airspeedSub(-1),
_controlState{},
_arming{},
_airspeed{},
_velocity_xy_filtered(0.0f),
_velocity_z_filtered(0.0f),
_airspeed_filtered(0.0f),
_accel_horz_lp(0.0f)
{
_paramHandle.maxVelocity = param_find("LNDFW_VEL_XY_MAX");
_paramHandle.maxClimbRate = param_find("LNDFW_VEL_Z_MAX");
_paramHandle.maxAirSpeed = param_find("LNDFW_AIRSPD_MAX");
_paramHandle.maxIntVelocity = param_find("LNDFW_VELI_MAX");
}
TEST(ParamTest, SimpleFind) {
_add_parameters();
param_t param = param_find("TEST_2");
ASSERT_NE(PARAM_INVALID, param) << "param_find did not find parameter";
int32_t value;
int result = param_get(param, &value);
ASSERT_EQ(0, result) << "param_get did not return parameter";
ASSERT_EQ(4, value) << "value of returned parameter does not match";
}
MulticopterLandDetector::MulticopterLandDetector() : LandDetector(),
_paramHandle(),
_params(),
_vehicleLocalPositionSub(-1),
_actuatorsSub(-1),
_armingSub(-1),
_parameterSub(-1),
_attitudeSub(-1),
_manualSub(-1),
_vehicleLocalPosition{},
_actuators{},
_arming{},
_vehicleAttitude{},
_landTimer(0)
{
_paramHandle.maxRotation = param_find("LNDMC_ROT_MAX");
_paramHandle.maxVelocity = param_find("LNDMC_XY_VEL_MAX");
_paramHandle.maxClimbRate = param_find("LNDMC_Z_VEL_MAX");
_paramHandle.maxThrottle = param_find("LNDMC_THR_MAX");
}
VtolLandDetector::VtolLandDetector() : MulticopterLandDetector(),
_paramHandle(),
_params(),
_airspeedSub(-1),
_parameterSub(-1),
_airspeed{},
_was_in_air(false),
_airspeed_filtered(0)
{
_paramHandle.maxAirSpeed = param_find("LNDFW_AIRSPD_MAX");
}
static int parameters_init(struct fw_rate_control_param_handles *h)
{
/* PID parameters */
h->rollrate_p = param_find("FW_ROLLR_P"); //TODO define rate params for fixed wing
h->rollrate_i = param_find("FW_ROLLR_I");
h->rollrate_awu = param_find("FW_ROLLR_AWU");
h->pitchrate_p = param_find("FW_PITCHR_P");
h->pitchrate_i = param_find("FW_PITCHR_I");
h->pitchrate_awu = param_find("FW_PITCHR_AWU");
h->yawrate_p = param_find("FW_YAWR_P");
h->yawrate_i = param_find("FW_YAWR_I");
h->yawrate_awu = param_find("FW_YAWR_AWU");
return OK;
}
