fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-01 12:10:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'upstream/master' into offboard2_externalsetpointmessages

Browse Source 
Conflicts:
	src/modules/mavlink/mavlink_main.cpp
This commit is contained in:
Thomas Gubler
2014-07-23 09:31:28 +02:00
parent 8bb1f9a4bf a7d2963e2b
commit 825b84fa3b
75 changed files with 2163 additions and 557 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+26 -18
View File
@@ -546,24 +546,19 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
}
break;
#if 0
/* Flight termination */
case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command
//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_local->armed && cmd->param3 > 0.5 && parachute_enabled) {
transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION);
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
case VEHICLE_CMD_DO_FLIGHTTERMINATION: {
if (cmd->param1 > 0.5f) {
//XXX update state machine?
armed_local->force_failsafe = true;
warnx("forcing failsafe");
} else {
/* reject parachute depoyment not armed */
cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
armed_local->force_failsafe = false;
warnx("disabling failsafe");
}
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
}
break;
#endif
case VEHICLE_CMD_DO_SET_HOME: {
bool use_current = cmd->param1 > 0.5f;
@@ -940,6 +935,11 @@ int commander_thread_main(int argc, char *argv[])
struct system_power_s system_power;
memset(&system_power, 0, sizeof(system_power));
/* Subscribe to actuator controls (outputs) */
int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
struct actuator_controls_s actuator_controls;
memset(&actuator_controls, 0, sizeof(actuator_controls));
control_status_leds(&status, &armed, true);
/* now initialized */
@@ -1221,13 +1221,17 @@ int commander_thread_main(int argc, char *argv[])
if (updated) {
orb_copy(ORB_ID(battery_status), battery_sub, &battery);
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
/* only consider battery voltage if system has been running 2s and battery voltage is valid */
if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) {
status.battery_voltage = battery.voltage_filtered_v;
status.battery_current = battery.current_a;
status.condition_battery_voltage_valid = true;
status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah);
/* get throttle (if armed), as we only care about energy negative throttle also counts */
float throttle = (armed.armed) ? fabsf(actuator_controls.control[3]) : 0.0f;
status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah, throttle);
}
}
@@ -1289,13 +1293,13 @@ int commander_thread_main(int argc, char *argv[])
}
/* if battery voltage is getting lower, warn using buzzer, etc. */
if (status.condition_battery_voltage_valid && status.battery_remaining < 0.25f && !low_battery_voltage_actions_done) {
if (status.condition_battery_voltage_valid && status.battery_remaining < 0.18f && !low_battery_voltage_actions_done) {
low_battery_voltage_actions_done = true;
mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED");
status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
status_changed = true;
} else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
} else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.09f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
/* critical battery voltage, this is rather an emergency, change state machine */
critical_battery_voltage_actions_done = true;
mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY");
@@ -1434,8 +1438,12 @@ int commander_thread_main(int argc, char *argv[])
arming_state_changed = true;
} else if (arming_ret == TRANSITION_DENIED) {
/* DENIED here indicates bug in the commander */
mavlink_log_critical(mavlink_fd, "arming state transition denied");
/*
* the arming transition can be denied to a number of reasons:
* - pre-flight check failed (sensors not ok or not calibrated)
* - safety not disabled
* - system not in manual mode
*/
tune_negative(true);
}
src/modules/commander/commander_helper.cpp
+11 -6
View File
@@ -281,15 +281,17 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern)
}
}
float battery_remaining_estimate_voltage(float voltage, float discharged)
float battery_remaining_estimate_voltage(float voltage, float discharged, float throttle_normalized)
{
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 param_t bat_v_load_drop_h;
static float bat_v_empty = 3.4f;
static float bat_v_full = 4.2f;
static float bat_v_load_drop = 0.06f;
static int bat_n_cells = 3;
static float bat_capacity = -1.0f;
static bool initialized = false;
@@ -297,23 +299,26 @@ float battery_remaining_estimate_voltage(float voltage, float discharged)
if (!initialized) {
bat_v_empty_h = param_find("BAT_V_EMPTY");
bat_v_full_h = param_find("BAT_V_FULL");
bat_v_full_h = param_find("BAT_V_CHARGED");
bat_n_cells_h = param_find("BAT_N_CELLS");
bat_capacity_h = param_find("BAT_CAPACITY");
bat_v_load_drop_h = param_find("BAT_V_LOAD_DROP");
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_v_load_drop_h, &bat_v_load_drop);
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));
/* remaining charge estimate based on voltage and internal resistance (drop under load) */
float bat_v_full_dynamic = bat_v_full - (bat_v_load_drop * throttle_normalized);
float remaining_voltage = (voltage - (bat_n_cells * bat_v_empty)) / (bat_n_cells * (bat_v_full_dynamic - 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 */
src/modules/commander/commander_helper.h
+2 -1
View File
@@ -80,8 +80,9 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern);
*
* @param voltage the current battery voltage
* @param discharged the discharged capacity
* @param throttle_normalized the normalized throttle magnitude from 0 to 1. Negative throttle should be converted to this range as well, as it consumes energy.
* @return the estimated remaining capacity in 0..1
*/
float battery_remaining_estimate_voltage(float voltage, float discharged);
float battery_remaining_estimate_voltage(float voltage, float discharged, float throttle_normalized);
#endif /* COMMANDER_HELPER_H_ */
src/modules/commander/commander_params.c
+11 -1
View File
@@ -65,7 +65,17 @@ PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f);
*
* @group Battery Calibration
*/
PARAM_DEFINE_FLOAT(BAT_V_FULL, 3.9f);
PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.2f);
/**
* Voltage drop per cell on 100% load
*
* This implicitely defines the internal resistance
* to maximum current ratio and assumes linearity.
*
* @group Battery Calibration
*/
PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.07f);
/**
* Number of cells.
src/modules/commander/state_machine_helper.cpp
+17 -9
View File
@@ -110,8 +110,8 @@ arming_state_transition(struct vehicle_status_s *status, /// current
ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1);
transition_result_t ret = TRANSITION_DENIED;
arming_state_t current_arming_state = status->arming_state;
bool feedback_provided = false;
/* only check transition if the new state is actually different from the current one */
if (new_arming_state == current_arming_state) {
@@ -156,13 +156,15 @@ arming_state_transition(struct vehicle_status_s *status, /// current
// Fail transition if pre-arm check fails
if (prearm_ret) {
/* the prearm check already prints the reject reason */
feedback_provided = true;
valid_transition = false;
// Fail transition if we need safety switch press
} else if (safety->safety_switch_available && !safety->safety_off) {
mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch!");
mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first!");
feedback_provided = true;
valid_transition = false;
}
@@ -173,6 +175,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current
if (!status->condition_power_input_valid) {
mavlink_log_critical(mavlink_fd, "NOT ARMING: Connect power module.");
feedback_provided = true;
valid_transition = false;
}
@@ -182,6 +185,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current
(status->avionics_power_rail_voltage < 4.9f))) {
mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage);
feedback_provided = true;
valid_transition = false;
}
}
@@ -200,6 +204,8 @@ arming_state_transition(struct vehicle_status_s *status, /// current
/* Sensors need to be initialized for STANDBY state */
if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) {
mavlink_log_critical(mavlink_fd, "NOT ARMING: Sensors not operational.");
feedback_provided = true;
valid_transition = false;
}
@@ -216,11 +222,14 @@ arming_state_transition(struct vehicle_status_s *status, /// current
}
if (ret == TRANSITION_DENIED) {
static const char *errMsg = "INVAL: %s - %s";
const char * str = "INVAL: %s - %s";
/* only print to console here by default as this is too technical to be useful during operation */
warnx(str, state_names[status->arming_state], state_names[new_arming_state]);
mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]);
warnx(errMsg, state_names[status->arming_state], state_names[new_arming_state]);
/* print to MAVLink if we didn't provide any feedback yet */
if (!feedback_provided) {
mavlink_log_critical(mavlink_fd, str, state_names[status->arming_state], state_names[new_arming_state]);
}
}
return ret;
@@ -648,8 +657,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL RANGE");
mavlink_log_critical(mavlink_fd, "hold still while arming");
mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL RANGE, hold still");
/* this is frickin' fatal */
failed = true;
goto system_eval;