Skip to content

Commit

Permalink
Merge branch 'main' into patch-14
Browse files Browse the repository at this point in the history
  • Loading branch information
chfriedrich98 authored Jul 5, 2024
2 parents b5e7546 + a1f4363 commit c4fd4bc
Show file tree
Hide file tree
Showing 43 changed files with 1,222 additions and 468 deletions.
1 change: 1 addition & 0 deletions ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@
#
# @maintainer
# @board px4_fmu-v2 exclude
# @board px4_fmu-v6x exclude
#

. ${R}etc/init.d/rc.mc_defaults
Expand Down
1 change: 1 addition & 0 deletions ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@
# @board px4_fmu-v4pro exclude
# @board px4_fmu-v5 exclude
# @board px4_fmu-v5x exclude
# @board px4_fmu-v6x exclude
# @board diatone_mamba-f405-mk2 exclude
#
. ${R}etc/init.d/rc.mc_defaults
Expand Down
26 changes: 16 additions & 10 deletions ROMFS/px4fmu_common/init.d/rcS
Original file line number Diff line number Diff line change
Expand Up @@ -217,25 +217,31 @@ else
fi
unset BOARD_RC_DEFAULTS

#
# Set parameters and env variables for selected SYS_AUTOSTART.
#
set AUTOSTART_PATH etc/init.d/rc.autostart
# Load airframe configuration based on SYS_AUTOSTART parameter
if ! param compare SYS_AUTOSTART 0
then
if param greater SYS_AUTOSTART 1000000
# rc.autostart directly run the right airframe script which sets the VEHICLE_TYPE
# Look for airframe in ROMFS
. ${R}etc/init.d/rc.autostart

if [ ${VEHICLE_TYPE} == none ]
then
# Use external startup file
# Look for airframe on SD card
if [ $SDCARD_AVAILABLE = yes ]
then
set AUTOSTART_PATH etc/init.d/rc.autostart_ext
. ${R}etc/init.d/rc.autostart_ext
else
echo "ERROR [init] SD card not mounted - trying to load airframe from ROMFS"
echo "ERROR [init] SD card not mounted - can't load external airframe"
fi
fi
. ${R}$AUTOSTART_PATH

if [ ${VEHICLE_TYPE} == none ]
then
echo "ERROR [init] No airframe file found for SYS_AUTOSTART value"
param set SYS_AUTOSTART 0
tune_control play error
fi
fi
unset AUTOSTART_PATH

# Check parameter version and reset upon airframe configuration version mismatch.
# Reboot required because "param reset_all" would reset all "param set" lines from airframe.
Expand Down
6 changes: 0 additions & 6 deletions Tools/px4airframes/rcout.py
Original file line number Diff line number Diff line change
Expand Up @@ -79,12 +79,6 @@ def __init__(self, groups, board, post_start=False):
result += "then\n"
result += "\techo \"Loading airframe: /etc/init.d/airframes/${AIRFRAME}\"\n"
result += "\t. /etc/init.d/airframes/${AIRFRAME}\n"
if not post_start:
result += "else\n"
result += "\techo \"ERROR [init] No file matches SYS_AUTOSTART value found in : /etc/init.d/airframes\"\n"
# Reset the configuration
result += "\tparam set SYS_AUTOSTART 0\n"
result += "\ttone_alarm ${TUNE_ERR}\n"
result += "fi\n"
result += "unset AIRFRAME"
self.output = result
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -1148,7 +1148,6 @@ handle_message_hil_sensor_dsp(mavlink_message_t *msg)

hil_battery_status.timestamp = gyro_accel_time;
hil_battery_status.voltage_v = 16.0f;
hil_battery_status.voltage_filtered_v = 16.0f;
hil_battery_status.current_a = 10.0f;
hil_battery_status.discharged_mah = -1.0f;
hil_battery_status.connected = true;
Expand Down
6 changes: 0 additions & 6 deletions msg/BatteryStatus.msg
Original file line number Diff line number Diff line change
@@ -1,9 +1,7 @@
uint64 timestamp # time since system start (microseconds)
bool connected # Whether or not a battery is connected, based on a voltage threshold
float32 voltage_v # Battery voltage in volts, 0 if unknown
float32 voltage_filtered_v # Battery voltage in volts, filtered, 0 if unknown
float32 current_a # Battery current in amperes, -1 if unknown
float32 current_filtered_a # Battery current in amperes, filtered, 0 if unknown
float32 current_average_a # Battery current average in amperes (for FW average in level flight), -1 if unknown
float32 discharged_mah # Discharged amount in mAh, -1 if unknown
float32 remaining # From 1 to 0, -1 if unknown
Expand Down Expand Up @@ -68,12 +66,8 @@ uint8 BATTERY_MODE_COUNT = 3 # Counter - keep it as last element (once we're ful

uint8 MAX_INSTANCES = 4

float32 average_power # The average power of the current discharge
float32 available_energy # The predicted charge or energy remaining in the battery
float32 full_charge_capacity_wh # The compensated battery capacity
float32 remaining_capacity_wh # The compensated battery capacity remaining
float32 design_capacity # The design capacity of the battery
uint16 average_time_to_full # The predicted remaining time until the battery reaches full charge, in minutes
uint16 over_discharge_count # Number of battery overdischarge
float32 nominal_voltage # Nominal voltage of the battery pack

Expand Down
2 changes: 0 additions & 2 deletions src/drivers/batt_smbus/batt_smbus.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -116,13 +116,11 @@ void BATT_SMBUS::RunImpl()

// Convert millivolts to volts.
new_report.voltage_v = ((float)result) / 1000.0f;
new_report.voltage_filtered_v = new_report.voltage_v;

// Read current.
ret |= _interface->read_word(BATT_SMBUS_CURRENT, result);

new_report.current_a = (-1.0f * ((float)(*(int16_t *)&result)) / 1000.0f) * _c_mult;
new_report.current_filtered_a = new_report.current_a;

// Read average current.
ret |= _interface->read_word(BATT_SMBUS_AVERAGE_CURRENT, result);
Expand Down
Loading

0 comments on commit c4fd4bc

Please sign in to comment.