Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-09-28 15:51:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' into autostart_cleanup

Browse Source
This commit is contained in:
Anton Babushkin
2014-01-14 15:52:46 +01:00
parent 1008d0c383 b529e112b8
commit 7b60761bf5
27 changed files with 865 additions and 189 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d/rcS
+24 -14
View File
@@ -59,20 +59,6 @@ fi
if [ $MODE == autostart ]
then
echo "[init] AUTOSTART mode"
#
# Load parameters
#
param select /fs/microsd/params
if [ -f /fs/microsd/params ]
then
if param load /fs/microsd/params
then
echo "[init] Parameters loaded"
else
echo "[init] Parameter file corrupt - ignoring"
fi
fi
#
# Start CDC/ACM serial driver
@@ -84,6 +70,30 @@ then
#
uorb start
#
# Load parameters
#
if mtd start
then
param select /fs/mtd_params
if param load /fs/mtd_params
then
else
echo "FAILED LOADING PARAMS"
fi
else
param select /fs/microsd/params
if [ -f /fs/microsd/params ]
then
if param load /fs/microsd/params
then
echo "Parameters loaded"
else
echo "Parameter file corrupt - ignoring"
fi
fi
fi
#
# Start system state indicator
#
Tools/px4params/dokuwikiout.py
+20 -10
View File
@@ -5,23 +5,33 @@ class DokuWikiOutput(output.Output):
result = ""
for group in groups:
result += "==== %s ====\n\n" % group.GetName()
result += "^ Name ^ Description ^ Min ^ Max ^ Default ^ Comment ^\n"
for param in group.GetParams():
code = param.GetFieldValue("code")
name = param.GetFieldValue("short_desc")
if code != name:
name = "%s (%s)" % (name, code)
result += "=== %s ===\n\n" % name
long_desc = param.GetFieldValue("long_desc")
if long_desc is not None:
result += "%s\n\n" % long_desc
name = name.replace("\n", "")
result += "| %s | %s " % (code, name)
min_val = param.GetFieldValue("min")
if min_val is not None:
result += "* Minimal value: %s\n" % min_val
result += "| %s " % min_val
else:
result += "|"
max_val = param.GetFieldValue("max")
if max_val is not None:
result += "* Maximal value: %s\n" % max_val
result += "| %s " % max_val
else:
result += "|"
def_val = param.GetFieldValue("default")
if def_val is not None:
result += "* Default value: %s\n" % def_val
result += "\n"
result += "| %s " % def_val
else:
result += "|"
long_desc = param.GetFieldValue("long_desc")
if long_desc is not None:
long_desc = long_desc.replace("\n", "")
result += "| %s " % long_desc
else:
result += "|"
result += "|\n"
result += "\n"
return result
Tools/px4params/dokuwikiout_listings.py
+27
View File
@@ -0,0 +1,27 @@
import output
class DokuWikiOutput(output.Output):
def Generate(self, groups):
result = ""
for group in groups:
result += "==== %s ====\n\n" % group.GetName()
for param in group.GetParams():
code = param.GetFieldValue("code")
name = param.GetFieldValue("short_desc")
if code != name:
name = "%s (%s)" % (name, code)
result += "=== %s ===\n\n" % name
long_desc = param.GetFieldValue("long_desc")
if long_desc is not None:
result += "%s\n\n" % long_desc
min_val = param.GetFieldValue("min")
if min_val is not None:
result += "* Minimal value: %s\n" % min_val
max_val = param.GetFieldValue("max")
if max_val is not None:
result += "* Maximal value: %s\n" % max_val
def_val = param.GetFieldValue("default")
if def_val is not None:
result += "* Default value: %s\n" % def_val
result += "\n"
return result
makefiles/config_px4fmu-v2_default.mk
+1
View File
@@ -60,6 +60,7 @@ MODULES += systemcmds/top
MODULES += systemcmds/tests
MODULES += systemcmds/config
MODULES += systemcmds/nshterm
MODULES += systemcmds/mtd
MODULES += systemcmds/hw_ver
#
makefiles/config_px4fmu-v2_test.mk
+1
View File
@@ -22,6 +22,7 @@ MODULES += systemcmds/perf
MODULES += systemcmds/reboot
MODULES += systemcmds/tests
MODULES += systemcmds/nshterm
MODULES += systemcmds/mtd
MODULES += systemcmds/hw_ver
#
nuttx-configs/px4fmu-v1/nsh/defconfig
+20 -1
View File
@@ -460,7 +460,7 @@ CONFIG_MMCSD_NSLOTS=1
CONFIG_MMCSD_SPI=y
CONFIG_MMCSD_SPICLOCK=24000000
# CONFIG_MMCSD_SDIO is not set
# CONFIG_MTD is not set
CONFIG_MTD=y
CONFIG_PIPES=y
# CONFIG_PM is not set
# CONFIG_POWER is not set
@@ -482,6 +482,25 @@ CONFIG_USART1_SERIAL_CONSOLE=y
# CONFIG_USART6_SERIAL_CONSOLE is not set
# CONFIG_NO_SERIAL_CONSOLE is not set
#
# MTD Configuration
#
CONFIG_MTD_PARTITION=y
CONFIG_MTD_BYTE_WRITE=y
#
# MTD Device Drivers
#
# CONFIG_RAMMTD is not set
# CONFIG_MTD_AT24XX is not set
# CONFIG_MTD_AT45DB is not set
# CONFIG_MTD_M25P is not set
# CONFIG_MTD_SMART is not set
# CONFIG_MTD_RAMTRON is not set
# CONFIG_MTD_SST25 is not set
# CONFIG_MTD_SST39FV is not set
# CONFIG_MTD_W25 is not set
#
# USART1 Configuration
#
nuttx-configs/px4fmu-v2/nsh/defconfig
+2 -2
View File
@@ -500,8 +500,8 @@ CONFIG_MTD=y
#
# MTD Configuration
#
# CONFIG_MTD_PARTITION is not set
# CONFIG_MTD_BYTE_WRITE is not set
CONFIG_MTD_PARTITION=y
CONFIG_MTD_BYTE_WRITE=y
#
# MTD Device Drivers
src/drivers/hil/hil.cpp
+5 -4
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2014 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
@@ -193,9 +193,10 @@ HIL::~HIL()
} while (_task != -1);
}
/* clean up the alternate device node */
if (_primary_pwm_device)
unregister_driver(PWM_OUTPUT_DEVICE_PATH);
// XXX already claimed with CDEV
// /* clean up the alternate device node */
// if (_primary_pwm_device)
// unregister_driver(PWM_OUTPUT_DEVICE_PATH);
g_hil = nullptr;
}
src/drivers/rgbled/rgbled.cpp
+9 -3
View File
@@ -559,7 +559,7 @@ RGBLED::get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b)
void
rgbled_usage()
{
warnx("missing command: try 'start', 'test', 'info', 'off', 'rgb 30 40 50'");
warnx("missing command: try 'start', 'test', 'info', 'off', 'stop', 'rgb 30 40 50'");
warnx("options:");
warnx(" -b i2cbus (%d)", PX4_I2C_BUS_LED);
warnx(" -a addr (0x%x)", ADDR);
@@ -643,7 +643,7 @@ rgbled_main(int argc, char *argv[])
if (g_rgbled == nullptr) {
warnx("not started");
rgbled_usage();
exit(0);
exit(1);
}
if (!strcmp(verb, "test")) {
@@ -669,7 +669,7 @@ rgbled_main(int argc, char *argv[])
exit(0);
}
if (!strcmp(verb, "off")) {
if (!strcmp(verb, "off") || !strcmp(verb, "stop")) {
fd = open(RGBLED_DEVICE_PATH, 0);
if (fd == -1) {
@@ -681,6 +681,12 @@ rgbled_main(int argc, char *argv[])
exit(ret);
}
if (!strcmp(verb, "stop")) {
delete g_rgbled;
g_rgbled = nullptr;
exit(0);
}
if (!strcmp(verb, "rgb")) {
if (argc < 5) {
errx(1, "Usage: rgbled rgb <red> <green> <blue>");
src/modules/commander/commander.cpp
+19 -46
View File
@@ -107,14 +107,9 @@ static const int ERROR = -1;
extern struct system_load_s system_load;
#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f
#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f
/* Decouple update interval and hysteris counters, all depends on intervals */
#define COMMANDER_MONITORING_INTERVAL 50000
#define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f))
#define LOW_VOLTAGE_BATTERY_COUNTER_LIMIT (LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
#define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
#define MAVLINK_OPEN_INTERVAL 50000
@@ -666,8 +661,6 @@ int commander_thread_main(int argc, char *argv[])
/* Start monitoring loop */
unsigned counter = 0;
unsigned low_voltage_counter = 0;
unsigned critical_voltage_counter = 0;
unsigned stick_off_counter = 0;
unsigned stick_on_counter = 0;
@@ -745,7 +738,6 @@ int commander_thread_main(int argc, char *argv[])
int battery_sub = orb_subscribe(ORB_ID(battery_status));
struct battery_status_s battery;
memset(&battery, 0, sizeof(battery));
battery.voltage_v = 0.0f;
/* Subscribe to subsystem info topic */
int subsys_sub = orb_subscribe(ORB_ID(subsystem_info));
@@ -890,14 +882,12 @@ int commander_thread_main(int argc, char *argv[])
if (updated) {
orb_copy(ORB_ID(battery_status), battery_sub, &battery);
// warnx("bat v: %2.2f", battery.voltage_v);
/* only consider battery voltage if system has been running 2s and battery voltage is higher than 4V */
if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_v > 4.0f) {
status.battery_voltage = battery.voltage_v;
/* 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(status.battery_voltage);
status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah);
}
}
@@ -950,46 +940,29 @@ int commander_thread_main(int argc, char *argv[])
//on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0);
}
// XXX remove later
//warnx("bat remaining: %2.2f", status.battery_remaining);
/* 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) {
//TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS
if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) {
low_battery_voltage_actions_done = true;
mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY");
status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
status_changed = true;
battery_tune_played = false;
}
low_voltage_counter++;
low_battery_voltage_actions_done = true;
mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY");
status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
status_changed = true;
battery_tune_played = false;
} else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
/* critical battery voltage, this is rather an emergency, change state machine */
if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) {
critical_battery_voltage_actions_done = true;
mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY");
status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
battery_tune_played = false;
critical_battery_voltage_actions_done = true;
mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY");
status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
battery_tune_played = false;
if (armed.armed) {
arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED_ERROR, &armed);
if (armed.armed) {
arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED_ERROR, &armed);
} else {
arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY_ERROR, &armed);
}
status_changed = true;
} else {
arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY_ERROR, &armed);
}
critical_voltage_counter++;
} else {
low_voltage_counter = 0;
critical_voltage_counter = 0;
status_changed = true;
}
/* End battery voltage check */
src/modules/commander/commander_helper.cpp
+28 -16
View File
@@ -44,6 +44,7 @@
#include <stdint.h>
#include <stdbool.h>
#include <fcntl.h>
#include <math.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
@@ -251,36 +252,47 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern)
ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern);
}
float battery_remaining_estimate_voltage(float voltage)
float battery_remaining_estimate_voltage(float voltage, float discharged)
{
float ret = 0;
static param_t bat_volt_empty;
static param_t bat_volt_full;
static param_t bat_n_cells;
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;
static float ncells = 3;
// XXX change cells to int (and param to INT32)
if (!initialized) {
bat_volt_empty = param_find("BAT_V_EMPTY");
bat_volt_full = param_find("BAT_V_FULL");
bat_n_cells = param_find("BAT_N_CELLS");
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;
}
static float chemistry_voltage_empty = 3.2f;
static float chemistry_voltage_full = 4.05f;
if (counter % 100 == 0) {
param_get(bat_volt_empty, &chemistry_voltage_empty);
param_get(bat_volt_full, &chemistry_voltage_full);
param_get(bat_n_cells, &ncells);
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++;
ret = (voltage - ncells * chemistry_voltage_empty) / (ncells * (chemistry_voltage_full - chemistry_voltage_empty));
/* 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;
src/modules/commander/commander_helper.h
+4 -3
View File
@@ -75,12 +75,13 @@ void rgbled_set_mode(rgbled_mode_t mode);
void rgbled_set_pattern(rgbled_pattern_t *pattern);
/**
* Provides a coarse estimate of remaining battery power.
* Estimate remaining battery charge.
*
* The estimate is very basic and based on decharging voltage curves.
* Use integral of current if battery capacity known (BAT_CAPACITY parameter set),
* else use simple estimate based on voltage.
*
* @return the estimated remaining capacity in 0..1
*/
float battery_remaining_estimate_voltage(float voltage);
float battery_remaining_estimate_voltage(float voltage, float discharged);
#endif /* COMMANDER_HELPER_H_ */
src/modules/commander/commander_params.c
+4 -3
View File
@@ -50,6 +50,7 @@ PARAM_DEFINE_FLOAT(NAV_TAKEOFF_GAP, 3.0f);
PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f);
PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f);
PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f);
PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f);
PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f);
PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3);
PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f);
PARAM_DEFINE_FLOAT(BAT_V_FULL, 3.9f);
PARAM_DEFINE_INT32(BAT_N_CELLS, 3);
PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f);
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+60 -7
View File
@@ -1,7 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
* Copyright (c) 2013, 2014 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
@@ -49,21 +48,75 @@
*
*/
/**
* L1 period
*
* This is the L1 distance and defines the tracking
* point ahead of the aircraft its following.
* A value of 25 meters works for most aircraft. Shorten
* slowly during tuning until response is sharp without oscillation.
*
* @min 1.0
* @max 100.0
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_L1_PERIOD, 25.0f);
/**
* L1 damping
*
* Damping factor for L1 control.
*
* @min 0.6
* @max 0.9
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_L1_DAMPING, 0.75f);
/**
* Default Loiter Radius
*
* This radius is used when no other loiter radius is set.
*
* @min 10.0
* @max 100.0
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_LOITER_R, 50.0f);
/**
* Cruise throttle
*
* This is the throttle setting required to achieve the desired cruise speed. Most airframes have a value of 0.5-0.7.
*
* @min 0.0
* @max 1.0
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_THR_CRUISE, 0.7f);
/**
* Negative pitch limit
*
* The minimum negative pitch the controller will output.
*
* @unit degrees
* @min -60.0
* @max 0.0
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_P_LIM_MIN, -45.0f);
/**
* Positive pitch limit
*
* The maximum positive pitch the controller will output.
*
* @unit degrees
* @min 0.0
* @max 60.0
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_P_LIM_MAX, 45.0f);
src/modules/mavlink/mavlink.c
+2 -2
View File
@@ -677,8 +677,8 @@ int mavlink_thread_main(int argc, char *argv[])
v_status.onboard_control_sensors_health,
v_status.load * 1000.0f,
v_status.battery_voltage * 1000.0f,
v_status.battery_current * 1000.0f,
v_status.battery_remaining,
v_status.battery_current * 100.0f,
v_status.battery_remaining * 100.0f,
v_status.drop_rate_comm,
v_status.errors_comm,
v_status.errors_count1,
src/modules/sdlog2/sdlog2.c
+24 -6
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
* Anton Babushkin <anton.babushkin@me.com>
*
@@ -702,6 +702,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct airspeed_s airspeed;
struct esc_status_s esc;
struct vehicle_global_velocity_setpoint_s global_vel_sp;
struct battery_status_s battery;
} buf;
memset(&buf, 0, sizeof(buf));
@@ -726,6 +727,7 @@ int sdlog2_thread_main(int argc, char *argv[])
int airspeed_sub;
int esc_sub;
int global_vel_sp_sub;
int battery_sub;
} subs;
/* log message buffer: header + body */
@@ -752,6 +754,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_GPSP_s log_GPSP;
struct log_ESC_s log_ESC;
struct log_GVSP_s log_GVSP;
struct log_BATT_s log_BATT;
} body;
} log_msg = {
LOG_PACKET_HEADER_INIT(0)
@@ -760,9 +763,9 @@ int sdlog2_thread_main(int argc, char *argv[])
memset(&log_msg.body, 0, sizeof(log_msg.body));
/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
/* number of subscriptions */
const ssize_t fdsc = 19;
/* sanity check variable and index */
/* number of messages */
const ssize_t fdsc = 25;
/* Sanity check variable and index */
ssize_t fdsc_count = 0;
/* file descriptors to wait for */
struct pollfd fds[fdsc];
@@ -881,6 +884,12 @@ int sdlog2_thread_main(int argc, char *argv[])
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- BATTERY --- */
subs.battery_sub = orb_subscribe(ORB_ID(battery_status));
fds[fdsc_count].fd = subs.battery_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* WARNING: If you get the error message below,
* then the number of registered messages (fdsc)
* differs from the number of messages in the above list.
@@ -971,8 +980,6 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state;
log_msg.body.log_STAT.navigation_state = (uint8_t) buf_status.navigation_state;
log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state;
log_msg.body.log_STAT.battery_voltage = buf_status.battery_voltage;
log_msg.body.log_STAT.battery_current = buf_status.battery_current;
log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining;
log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning;
log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed;
@@ -1238,6 +1245,17 @@ int sdlog2_thread_main(int argc, char *argv[])
LOGBUFFER_WRITE_AND_COUNT(GVSP);
}
/* --- BATTERY --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(battery_status), subs.battery_sub, &buf.battery);
log_msg.msg_type = LOG_BATT_MSG;
log_msg.body.log_BATT.voltage = buf.battery.voltage_v;
log_msg.body.log_BATT.voltage_filtered = buf.battery.voltage_filtered_v;
log_msg.body.log_BATT.current = buf.battery.current_a;
log_msg.body.log_BATT.discharged = buf.battery.discharged_mah;
LOGBUFFER_WRITE_AND_COUNT(BATT);
}
/* signal the other thread new data, but not yet unlock */
if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) {
/* only request write if several packets can be written at once */
src/modules/sdlog2/sdlog2_messages.h
+11 -3
View File
@@ -148,8 +148,6 @@ struct log_STAT_s {
uint8_t main_state;
uint8_t navigation_state;
uint8_t arming_state;
float battery_voltage;
float battery_current;
float battery_remaining;
uint8_t battery_warning;
uint8_t landed;
@@ -248,6 +246,15 @@ struct log_GVSP_s {
float vz;
};
/* --- BATT - BATTERY --- */
#define LOG_BATT_MSG 20
struct log_BATT_s {
float voltage;
float voltage_filtered;
float current;
float discharged;
};
/* --- TIME - TIME STAMP --- */
#define LOG_TIME_MSG 129
struct log_TIME_s {
@@ -281,7 +288,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"),
LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"),
LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
LOG_FORMAT(STAT, "BBBfffBB", "MainState,NavState,ArmState,BatV,BatC,BatRem,BatWarn,Landed"),
LOG_FORMAT(STAT, "BBBfBB", "MainState,NavState,ArmState,BatRem,BatWarn,Landed"),
LOG_FORMAT(RC, "ffffffffB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"),
LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"),
@@ -291,6 +298,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(GPSP, "BLLfffbBffff", "AltRel,Lat,Lon,Alt,Yaw,LoiterR,LoiterDir,NavCmd,P1,P2,P3,P4"),
LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"),
/* system-level messages, ID >= 0x80 */
// FMT: don't write format of format message, it's useless
LOG_FORMAT(TIME, "Q", "StartTime"),
src/modules/sensors/module.mk
+1 -1
View File
@@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
# Copyright (c) 2012-2014 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
src/modules/sensors/sensor_params.c
+172 -15
View File
File diff suppressed because it is too large Load Diff
src/modules/sensors/sensors.cpp
+62 -27
View File
@@ -114,6 +114,7 @@
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
#define ADC_BATTERY_VOLTAGE_CHANNEL 10
#define ADC_BATTERY_CURRENT_CHANNEL -1
#define ADC_AIRSPEED_VOLTAGE_CHANNEL 11
#endif
@@ -124,10 +125,8 @@
#define ADC_AIRSPEED_VOLTAGE_CHANNEL 15
#endif
#define BAT_VOL_INITIAL 0.f
#define BAT_VOL_LOWPASS_1 0.99f
#define BAT_VOL_LOWPASS_2 0.01f
#define VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS 3.5f
#define BATT_V_LOWPASS 0.001f
#define BATT_V_IGNORE_THRESHOLD 3.5f
/**
* HACK - true temperature is much less than indicated temperature in baro,
@@ -215,6 +214,9 @@ private:
math::Matrix _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */
bool _mag_is_external; /**< true if the active mag is on an external board */
uint64_t _battery_discharged; /**< battery discharged current in mA*ms */
hrt_abstime _battery_current_timestamp; /**< timestamp of last battery current reading */
struct {
float min[_rc_max_chan_count];
float trim[_rc_max_chan_count];
@@ -265,6 +267,7 @@ private:
float rc_fs_thr;
float battery_voltage_scaling;
float battery_current_scaling;
} _parameters; /**< local copies of interesting parameters */
@@ -314,6 +317,7 @@ private:
param_t rc_fs_thr;
param_t battery_voltage_scaling;
param_t battery_current_scaling;
param_t board_rotation;
param_t external_mag_rotation;
@@ -467,7 +471,9 @@ Sensors::Sensors() :
_board_rotation(3, 3),
_external_mag_rotation(3, 3),
_mag_is_external(false)
_mag_is_external(false),
_battery_discharged(0),
_battery_current_timestamp(0)
{
/* basic r/c parameters */
@@ -560,6 +566,7 @@ Sensors::Sensors() :
_parameter_handles.diff_pres_analog_enabled = param_find("SENS_DPRES_ANA");
_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
_parameter_handles.battery_current_scaling = param_find("BAT_C_SCALING");
/* rotations */
_parameter_handles.board_rotation = param_find("SENS_BOARD_ROT");
@@ -740,6 +747,11 @@ Sensors::parameters_update()
warnx("Failed updating voltage scaling param");
}
/* scaling of ADC ticks to battery current */
if (param_get(_parameter_handles.battery_current_scaling, &(_parameters.battery_current_scaling)) != OK) {
warnx("Failed updating current scaling param");
}
param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation));
param_get(_parameter_handles.external_mag_rotation, &(_parameters.external_mag_rotation));
@@ -1157,17 +1169,16 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
if (!_publishing)
return;
hrt_abstime t = hrt_absolute_time();
/* rate limit to 100 Hz */
if (hrt_absolute_time() - _last_adc >= 10000) {
if (t - _last_adc >= 10000) {
/* make space for a maximum of eight channels */
struct adc_msg_s buf_adc[8];
/* read all channels available */
int ret = read(_fd_adc, &buf_adc, sizeof(buf_adc));
for (unsigned i = 0; i < sizeof(buf_adc) / sizeof(buf_adc[0]); i++) {
if (ret >= (int)sizeof(buf_adc[0])) {
if (ret >= (int)sizeof(buf_adc[0])) {
for (unsigned i = 0; i < sizeof(buf_adc) / sizeof(buf_adc[0]); i++) {
/* Save raw voltage values */
if (i < (sizeof(raw.adc_voltage_v)) / sizeof(raw.adc_voltage_v[0])) {
raw.adc_voltage_v[i] = buf_adc[i].am_data / (4096.0f / 3.3f);
@@ -1178,27 +1189,40 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
/* Voltage in volts */
float voltage = (buf_adc[i].am_data * _parameters.battery_voltage_scaling);
if (voltage > VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) {
if (voltage > BATT_V_IGNORE_THRESHOLD) {
_battery_status.voltage_v = voltage;
/* one-time initialization of low-pass value to avoid long init delays */
if (_battery_status.voltage_v < 3.0f) {
_battery_status.voltage_v = voltage;
if (_battery_status.voltage_filtered_v < BATT_V_IGNORE_THRESHOLD) {
_battery_status.voltage_filtered_v = voltage;
}
_battery_status.timestamp = hrt_absolute_time();
_battery_status.voltage_v = (BAT_VOL_LOWPASS_1 * (_battery_status.voltage_v + BAT_VOL_LOWPASS_2 * voltage));;
/* current and discharge are unknown */
_battery_status.current_a = -1.0f;
_battery_status.discharged_mah = -1.0f;
_battery_status.timestamp = t;
_battery_status.voltage_filtered_v += (voltage - _battery_status.voltage_filtered_v) * BATT_V_LOWPASS;
/* announce the battery voltage if needed, just publish else */
if (_battery_pub > 0) {
orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status);
} else {
/* mark status as invalid */
_battery_status.voltage_v = -1.0f;
_battery_status.voltage_filtered_v = -1.0f;
}
} else {
_battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status);
} else if (ADC_BATTERY_CURRENT_CHANNEL == buf_adc[i].am_channel) {
/* handle current only if voltage is valid */
if (_battery_status.voltage_v > 0.0f) {
float current = (buf_adc[i].am_data * _parameters.battery_current_scaling);
/* check measured current value */
if (current >= 0.0f) {
_battery_status.timestamp = t;
_battery_status.current_a = current;
if (_battery_current_timestamp != 0) {
/* initialize discharged value */
if (_battery_status.discharged_mah < 0.0f)
_battery_status.discharged_mah = 0.0f;
_battery_discharged += current * (t - _battery_current_timestamp);
_battery_status.discharged_mah = ((float) _battery_discharged) / 3600000.0f;
}
}
}
_battery_current_timestamp = t;
} else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) {
@@ -1214,7 +1238,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
float diff_pres_pa = voltage * 1000.0f - _parameters.diff_pres_offset_pa; //for MPXV7002DP sensor
_diff_pres.timestamp = hrt_absolute_time();
_diff_pres.timestamp = t;
_diff_pres.differential_pressure_pa = diff_pres_pa;
_diff_pres.voltage = voltage;
@@ -1227,8 +1251,16 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
}
}
}
}
_last_adc = t;
if (_battery_status.voltage_v > 0.0f) {
/* announce the battery status if needed, just publish else */
if (_battery_pub > 0) {
orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status);
_last_adc = hrt_absolute_time();
} else {
_battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status);
}
}
}
}
@@ -1516,7 +1548,10 @@ Sensors::task_main()
raw.adc_voltage_v[3] = 0.0f;
memset(&_battery_status, 0, sizeof(_battery_status));
_battery_status.voltage_v = BAT_VOL_INITIAL;
_battery_status.voltage_v = 0.0f;
_battery_status.voltage_filtered_v = 0.0f;
_battery_status.current_a = -1.0f;
_battery_status.discharged_mah = -1.0f;
/* get a set of initial values */
accel_poll(raw);
src/modules/systemlib/bson/tinybson.c
+3
View File
@@ -407,6 +407,9 @@ bson_encoder_fini(bson_encoder_t encoder)
memcpy(encoder->buf, &len, sizeof(len));
}
/* sync file */
fsync(encoder->fd);
return 0;
}
src/modules/systemlib/param/param.c
+29 -6
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2014 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
@@ -61,7 +61,7 @@
#include "uORB/uORB.h"
#include "uORB/topics/parameter_update.h"
#if 1
#if 0
# define debug(fmt, args...) do { warnx(fmt, ##args); } while(0)
#else
# define debug(fmt, args...) do { } while(0)
@@ -512,6 +512,28 @@ param_save_default(void)
int fd;
const char *filename = param_get_default_file();
/* write parameters to temp file */
fd = open(filename, O_WRONLY | O_CREAT);
if (fd < 0) {
warn("failed to open param file: %s", filename);
return ERROR;
}
if (res == OK) {
res = param_export(fd, false);
if (res != OK) {
warnx("failed to write parameters to file: %s", filename);
}
}
close(fd);
return res;
#if 0
const char *filename_tmp = malloc(strlen(filename) + 5);
sprintf(filename_tmp, "%s.tmp", filename);
@@ -565,6 +587,7 @@ param_save_default(void)
free(filename_tmp);
return res;
#endif
}
/**
@@ -573,9 +596,9 @@ param_save_default(void)
int
param_load_default(void)
{
int fd = open(param_get_default_file(), O_RDONLY);
int fd_load = open(param_get_default_file(), O_RDONLY);
if (fd < 0) {
if (fd_load < 0) {
/* no parameter file is OK, otherwise this is an error */
if (errno != ENOENT) {
warn("open '%s' for reading failed", param_get_default_file());
@@ -584,8 +607,8 @@ param_load_default(void)
return 1;
}
int result = param_load(fd);
close(fd);
int result = param_load(fd_load);
close(fd_load);
if (result != 0) {
warn("error reading parameters from '%s'", param_get_default_file());
src/modules/uORB/topics/battery_status.h
+5 -4
View File
@@ -53,9 +53,10 @@
*/
struct battery_status_s {
uint64_t timestamp; /**< microseconds since system boot, needed to integrate */
float voltage_v; /**< Battery voltage in volts, filtered */
float current_a; /**< Battery current in amperes, filtered, -1 if unknown */
float discharged_mah; /**< Discharged amount in mAh, filtered, -1 if unknown */
float voltage_v; /**< Battery voltage in volts, 0 if unknown */
float voltage_filtered_v; /**< Battery voltage in volts, filtered, 0 if unknown */
float current_a; /**< Battery current in amperes, -1 if unknown */
float discharged_mah; /**< Discharged amount in mAh, -1 if unknown */
};
/**
@@ -65,4 +66,4 @@ struct battery_status_s {
/* register this as object request broker structure */
ORB_DECLARE(battery_status);
#endif
#endif
src/systemcmds/mtd/module.mk
+6
View File
@@ -0,0 +1,6 @@
#
# RAMTRON file system driver
#
MODULE_COMMAND = mtd
SRCS = mtd.c
src/systemcmds/mtd/mtd.c
+279
View File
File diff suppressed because it is too large Load Diff
src/systemcmds/param/param.c
+38 -9
View File
@@ -72,7 +72,12 @@ param_main(int argc, char *argv[])
if (argc >= 3) {
do_save(argv[2]);
} else {
do_save(param_get_default_file());
if (param_save_default()) {
warnx("Param export failed.");
exit(1);
} else {
exit(0);
}
}
}
@@ -133,11 +138,8 @@ param_main(int argc, char *argv[])
static void
do_save(const char* param_file_name)
{
/* delete the parameter file in case it exists */
unlink(param_file_name);
/* create the file */
int fd = open(param_file_name, O_WRONLY | O_CREAT | O_EXCL);
int fd = open(param_file_name, O_WRONLY | O_CREAT);
if (fd < 0)
err(1, "opening '%s' failed", param_file_name);
@@ -146,7 +148,7 @@ do_save(const char* param_file_name)
close(fd);
if (result < 0) {
unlink(param_file_name);
(void)unlink(param_file_name);
errx(1, "error exporting to '%s'", param_file_name);
}
@@ -203,11 +205,38 @@ do_show_print(void *arg, param_t param)
int32_t i;
float f;
const char *search_string = (const char*)arg;
const char *p_name = (const char*)param_name(param);
/* print nothing if search string is invalid and not matching */
if (!(arg == NULL || (!strcmp(search_string, param_name(param))))) {
/* param not found */
return;
if (!(arg == NULL)) {
/* start search */
char *ss = search_string;
char *pp = p_name;
bool mismatch = false;
/* XXX this comparison is only ok for trailing wildcards */
while (*ss != '\0' && *pp != '\0') {
if (*ss == *pp) {
ss++;
pp++;
} else if (*ss == '*') {
if (*(ss + 1) != '\0') {
warnx("* symbol only allowed at end of search string.");
exit(1);
}
pp++;
} else {
/* param not found */
return;
}
}
/* the search string must have been consumed */
if (!(*ss == '\0' || *ss == '*'))
return;
}
printf("%c %s: ",
src/systemcmds/tests/test_file.c
+9 -7
View File
@@ -54,9 +54,9 @@
#include "tests.h"
int check_user_abort();
int check_user_abort(int fd);
int check_user_abort() {
int check_user_abort(int fd) {
/* check if user wants to abort */
char c;
@@ -77,6 +77,8 @@ int check_user_abort() {
case 'q':
{
warnx("Test aborted.");
fsync(fd);
close(fd);
return OK;
/* not reached */
}
@@ -141,7 +143,7 @@ test_file(int argc, char *argv[])
fsync(fd);
if (!check_user_abort())
if (!check_user_abort(fd))
return OK;
}
@@ -175,7 +177,7 @@ test_file(int argc, char *argv[])
return 1;
}
if (!check_user_abort())
if (!check_user_abort(fd))
return OK;
}
@@ -199,7 +201,7 @@ test_file(int argc, char *argv[])
return 1;
}
if (!check_user_abort())
if (!check_user_abort(fd))
return OK;
}
@@ -232,7 +234,7 @@ test_file(int argc, char *argv[])
break;
}
if (!check_user_abort())
if (!check_user_abort(fd))
return OK;
}
@@ -275,7 +277,7 @@ test_file(int argc, char *argv[])
break;
}
if (!check_user_abort())
if (!check_user_abort(fd))
return OK;
}