diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing index 708c34491b..3c2312fc7d 100644 --- a/ROMFS/px4fmu_common/init.d/3033_wingwing +++ b/ROMFS/px4fmu_common/init.d/3033_wingwing @@ -23,12 +23,7 @@ then param set FW_LND_TLALT 5 param set FW_THR_LND_MAX 0 param set FW_PR_FF 0.35 - param set FW_PR_I 0.005 - param set FW_PR_IMAX 0.4 - param set FW_PR_P 0.08 param set FW_RR_FF 0.6 - param set FW_RR_I 0.005 - param set FW_RR_IMAX 0.2 param set FW_RR_P 0.04 fi diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 9eac930589..0ffaea5bec 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -110,6 +110,7 @@ static float bat_v_load_drop = 0.06f; static int bat_n_cells = 3; static float bat_capacity = -1.0f; static unsigned int counter = 0; +static float throttle_lowpassed = 0.0f; int battery_init() { @@ -383,8 +384,15 @@ float battery_remaining_estimate_voltage(float voltage, float discharged, float counter++; + // XXX this time constant needs to become tunable + // but really, the right fix are smart batteries. + float val = throttle_lowpassed * 0.97f + throttle_normalized * 0.03f; + if (isfinite(val)) { + throttle_lowpassed = val; + } + /* remaining charge estimate based on voltage and internal resistance (drop under load) */ - float bat_v_empty_dynamic = bat_v_empty - (bat_v_load_drop * throttle_normalized); + float bat_v_empty_dynamic = bat_v_empty - (bat_v_load_drop * throttle_lowpassed); /* the range from full to empty is the same for batteries under load and without load, * since the voltage drop applies to both the full and empty state */ diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 7cf04594c6..371fadb2fc 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -808,6 +808,14 @@ FixedwingAttitudeControl::task_main() continue; } + /* default flaps to center */ + float flaps_control = 0.0f; + + /* map flaps by default to manual if valid */ + if (isfinite(_manual.flaps)) { + flaps_control = _manual.flaps; + } + /* decide if in stabilized or full manual control */ if (_vcontrol_mode.flag_control_attitude_enabled) { @@ -927,7 +935,6 @@ FixedwingAttitudeControl::task_main() /* allow manual control of rudder deflection */ yaw_manual = _manual.r; throttle_sp = _manual.z; - _actuators.control[4] = _manual.flaps; /* * in manual mode no external source should / does emit attitude setpoints. @@ -1090,13 +1097,13 @@ FixedwingAttitudeControl::task_main() } else { /* manual/direct control */ - _actuators.control[0] = _manual.y + _parameters.trim_roll; - _actuators.control[1] = -_manual.x + _parameters.trim_pitch; - _actuators.control[2] = _manual.r + _parameters.trim_yaw; - _actuators.control[3] = _manual.z; - _actuators.control[4] = _manual.flaps; + _actuators.control[actuator_controls_s::INDEX_ROLL] = _manual.y + _parameters.trim_roll; + _actuators.control[actuator_controls_s::INDEX_PITCH] = -_manual.x + _parameters.trim_pitch; + _actuators.control[actuator_controls_s::INDEX_YAW] = _manual.r + _parameters.trim_yaw; + _actuators.control[actuator_controls_s::INDEX_THROTTLE] = _manual.z; } + _actuators.control[actuator_controls_s::INDEX_FLAPS] = flaps_control; _actuators.control[5] = _manual.aux1; _actuators.control[6] = _manual.aux2; _actuators.control[7] = _manual.aux3; diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index dbf40230ba..78b68be7db 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -64,7 +64,7 @@ * @max 1.0 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_ATT_TC, 0.5f); +PARAM_DEFINE_FLOAT(FW_ATT_TC, 0.4f); /** * Pitch rate proportional gain. @@ -76,7 +76,7 @@ PARAM_DEFINE_FLOAT(FW_ATT_TC, 0.5f); * @max 1.0 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_PR_P, 0.05f); +PARAM_DEFINE_FLOAT(FW_PR_P, 0.08f); /** * Pitch rate integrator gain. @@ -88,7 +88,7 @@ PARAM_DEFINE_FLOAT(FW_PR_P, 0.05f); * @max 50.0 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_PR_I, 0.0f); +PARAM_DEFINE_FLOAT(FW_PR_I, 0.005f); /** * Maximum positive / up pitch rate. @@ -126,7 +126,7 @@ PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 0.0f); * @max 1.0 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.2f); +PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.4f); /** * Roll to Pitch feedforward gain. @@ -161,7 +161,7 @@ PARAM_DEFINE_FLOAT(FW_RR_P, 0.05f); * @max 100.0 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_RR_I, 0.0f); +PARAM_DEFINE_FLOAT(FW_RR_I, 0.005f); /** * Roll Integrator Anti-Windup @@ -247,7 +247,7 @@ PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0.0f); * @max 10.0 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_RR_FF, 0.3f); +PARAM_DEFINE_FLOAT(FW_RR_FF, 0.5f); /** * Pitch rate feed forward diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 5720d7b57c..8b2437e40a 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -58,7 +58,7 @@ * @max 100.0 * @group L1 Control */ -PARAM_DEFINE_FLOAT(FW_L1_PERIOD, 25.0f); +PARAM_DEFINE_FLOAT(FW_L1_PERIOD, 20.0f); /** * L1 damping @@ -80,7 +80,7 @@ PARAM_DEFINE_FLOAT(FW_L1_DAMPING, 0.75f); * @max 1.0 * @group L1 Control */ -PARAM_DEFINE_FLOAT(FW_THR_CRUISE, 0.7f); +PARAM_DEFINE_FLOAT(FW_THR_CRUISE, 0.6f); /** * Throttle max slew rate @@ -123,10 +123,11 @@ PARAM_DEFINE_FLOAT(FW_P_LIM_MAX, 45.0f); * The maximum roll the controller will output. * * @unit degrees - * @min 0.0 + * @min 35.0 + * @max 65.0 * @group L1 Control */ -PARAM_DEFINE_FLOAT(FW_R_LIM, 45.0f); +PARAM_DEFINE_FLOAT(FW_R_LIM, 50.0f); /** * Throttle limit max @@ -151,6 +152,8 @@ PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f); * For aircraft with internal combustion engine this parameter should be set * for desired idle rpm. * + * @min 0.0 + * @max 1.0 * @group L1 Control */ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); @@ -161,6 +164,8 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); * This throttle value will be set as throttle limit at FW_LND_TLALT, * before arcraft will flare. * + * @min 0.0 + * @max 1.0 * @group L1 Control */ PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f); @@ -173,6 +178,8 @@ PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f); * distance to the desired altitude. Mostly used for takeoff waypoints / modes. * Set to zero to disable climbout mode (not recommended). * + * @min 0.0 + * @max 150.0 * @group L1 Control */ PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 25.0f); @@ -193,6 +200,8 @@ PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 25.0f); * FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or * FW_THR_MAX reduced. * + * @min 2.0 + * @max 10.0 * @group L1 Control */ PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index fca0e91cbb..6876f8dde8 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -547,7 +547,7 @@ protected: msg.onboard_control_sensors_health = status.onboard_control_sensors_health; msg.load = status.load * 1000.0f; msg.voltage_battery = status.battery_voltage * 1000.0f; - msg.current_battery = status.battery_current * 100.0f; + msg.current_battery = status.battery_current / 10.0f; msg.drop_rate_comm = status.drop_rate_comm; msg.errors_comm = status.errors_comm; msg.errors_count1 = status.errors_count1; @@ -572,7 +572,7 @@ protected: bat_msg.voltages[i] = 0; } } - bat_msg.current_battery = status.battery_current * 100.0f; + bat_msg.current_battery = status.battery_current / 10.0f; bat_msg.current_consumed = status.battery_discharged_mah; bat_msg.energy_consumed = -1.0f; bat_msg.battery_remaining = (status.battery_voltage > 0) ?