aboutsummaryrefslogtreecommitdiff
path: root/apps
diff options
context:
space:
mode:
authorLorenz Meier <lm@inf.ethz.ch>2013-03-16 14:58:59 -0700
committerLorenz Meier <lm@inf.ethz.ch>2013-03-16 14:58:59 -0700
commit1b16387a996dcad53c3179b053162f7fee543254 (patch)
tree5c3b5a06cc04a12aad4472b7190b8d09fd6e81bf /apps
parent6b947a67d07eadc7dc882edf4505377085979784 (diff)
parentde078b4525c3a7f34fc3955fb9fef3564f20d62a (diff)
downloadpx4-firmware-1b16387a996dcad53c3179b053162f7fee543254.tar.gz
px4-firmware-1b16387a996dcad53c3179b053162f7fee543254.tar.bz2
px4-firmware-1b16387a996dcad53c3179b053162f7fee543254.zip
Merge pull request #221 from PX4/const-sweep
Mark a number of things (most particularly the ROMFS) const to save RAM
Diffstat (limited to 'apps')
-rwxr-xr-xapps/attitude_estimator_ekf/attitude_estimator_ekf_main.c66
-rw-r--r--apps/drivers/blinkm/blinkm.cpp8
-rw-r--r--apps/mavlink/orb_listener.c82
-rw-r--r--apps/px4/tests/test_bson.c2
-rw-r--r--apps/systemlib/param/param.c2
5 files changed, 80 insertions, 80 deletions
diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
index 7ca77e513..bd972f03f 100755
--- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
+++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
@@ -72,38 +72,6 @@
__EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]);
-static unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
-
-static float dt = 0.005f;
-/* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */
-static float z_k[9] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 9.81f, 0.2f, -0.2f, 0.2f}; /**< Measurement vector */
-static float x_aposteriori_k[12]; /**< states */
-static float P_aposteriori_k[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
- 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
- 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0,
- 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0,
- 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0,
- 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0,
- 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0,
- 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0,
- 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0,
- 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 100.0f, 0, 0,
- 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0,
- 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f,
- }; /**< init: diagonal matrix with big values */
-
-static float x_aposteriori[12];
-static float P_aposteriori[144];
-
-/* output euler angles */
-static float euler[3] = {0.0f, 0.0f, 0.0f};
-
-static float Rot_matrix[9] = {1.f, 0, 0,
- 0, 1.f, 0,
- 0, 0, 1.f
- }; /**< init: identity matrix */
-
-
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int attitude_estimator_ekf_task; /**< Handle of deamon task / thread */
@@ -153,7 +121,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
attitude_estimator_ekf_task = task_spawn("attitude_estimator_ekf",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
- 12000,
+ 12400,
attitude_estimator_ekf_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0);
@@ -193,6 +161,38 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
*/
int attitude_estimator_ekf_thread_main(int argc, char *argv[])
{
+
+const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
+
+ float dt = 0.005f;
+/* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */
+ float z_k[9] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 9.81f, 0.2f, -0.2f, 0.2f}; /**< Measurement vector */
+ float x_aposteriori_k[12]; /**< states */
+ float P_aposteriori_k[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 100.0f, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f,
+ }; /**< init: diagonal matrix with big values */
+
+ float x_aposteriori[12];
+ float P_aposteriori[144];
+
+ /* output euler angles */
+ float euler[3] = {0.0f, 0.0f, 0.0f};
+
+ float Rot_matrix[9] = {1.f, 0, 0,
+ 0, 1.f, 0,
+ 0, 0, 1.f
+ }; /**< init: identity matrix */
+
// print text
printf("Extended Kalman Filter Attitude Estimator initialized..\n\n");
fflush(stdout);
diff --git a/apps/drivers/blinkm/blinkm.cpp b/apps/drivers/blinkm/blinkm.cpp
index 54c7d4266..56265995f 100644
--- a/apps/drivers/blinkm/blinkm.cpp
+++ b/apps/drivers/blinkm/blinkm.cpp
@@ -135,7 +135,7 @@ public:
virtual int setMode(int mode);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
- static const char *script_names[];
+ static const char *const script_names[];
private:
enum ScriptID {
@@ -219,7 +219,7 @@ namespace
}
/* list of script names, must match script ID numbers */
-const char *BlinkM::script_names[] = {
+const char *const BlinkM::script_names[] = {
"USER",
"RGB",
"WHITE_FLASH",
@@ -499,7 +499,7 @@ BlinkM::led()
for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) {
if(vehicle_status_raw.voltage_battery < num_of_cells * MAX_CELL_VOLTAGE) break;
}
- printf("<blinkm> cells found:%u\n", num_of_cells);
+ printf("<blinkm> cells found:%d\n", num_of_cells);
} else {
if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_WARNING) {
@@ -827,7 +827,7 @@ BlinkM::get_firmware_version(uint8_t version[2])
{
const uint8_t msg = 'Z';
- return transfer(&msg, sizeof(msg), version, sizeof(version));
+ return transfer(&msg, sizeof(msg), version, 2);
}
void blinkm_usage() {
diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c
index 7c34fb474..1dd3fc2d8 100644
--- a/apps/mavlink/orb_listener.c
+++ b/apps/mavlink/orb_listener.c
@@ -91,32 +91,32 @@ static uint64_t last_sensor_timestamp;
static void *uorb_receive_thread(void *arg);
struct listener {
- void (*callback)(struct listener *l);
+ void (*callback)(const struct listener *l);
int *subp;
uintptr_t arg;
};
-static void l_sensor_combined(struct listener *l);
-static void l_vehicle_attitude(struct listener *l);
-static void l_vehicle_gps_position(struct listener *l);
-static void l_vehicle_status(struct listener *l);
-static void l_rc_channels(struct listener *l);
-static void l_input_rc(struct listener *l);
-static void l_global_position(struct listener *l);
-static void l_local_position(struct listener *l);
-static void l_global_position_setpoint(struct listener *l);
-static void l_local_position_setpoint(struct listener *l);
-static void l_attitude_setpoint(struct listener *l);
-static void l_actuator_outputs(struct listener *l);
-static void l_actuator_armed(struct listener *l);
-static void l_manual_control_setpoint(struct listener *l);
-static void l_vehicle_attitude_controls(struct listener *l);
-static void l_debug_key_value(struct listener *l);
-static void l_optical_flow(struct listener *l);
-static void l_vehicle_rates_setpoint(struct listener *l);
-static void l_home(struct listener *l);
-
-struct listener listeners[] = {
+static void l_sensor_combined(const struct listener *l);
+static void l_vehicle_attitude(const struct listener *l);
+static void l_vehicle_gps_position(const struct listener *l);
+static void l_vehicle_status(const struct listener *l);
+static void l_rc_channels(const struct listener *l);
+static void l_input_rc(const struct listener *l);
+static void l_global_position(const struct listener *l);
+static void l_local_position(const struct listener *l);
+static void l_global_position_setpoint(const struct listener *l);
+static void l_local_position_setpoint(const struct listener *l);
+static void l_attitude_setpoint(const struct listener *l);
+static void l_actuator_outputs(const struct listener *l);
+static void l_actuator_armed(const struct listener *l);
+static void l_manual_control_setpoint(const struct listener *l);
+static void l_vehicle_attitude_controls(const struct listener *l);
+static void l_debug_key_value(const struct listener *l);
+static void l_optical_flow(const struct listener *l);
+static void l_vehicle_rates_setpoint(const struct listener *l);
+static void l_home(const struct listener *l);
+
+static const struct listener listeners[] = {
{l_sensor_combined, &mavlink_subs.sensor_sub, 0},
{l_vehicle_attitude, &mavlink_subs.att_sub, 0},
{l_vehicle_gps_position, &mavlink_subs.gps_sub, 0},
@@ -144,7 +144,7 @@ struct listener listeners[] = {
static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]);
void
-l_sensor_combined(struct listener *l)
+l_sensor_combined(const struct listener *l)
{
struct sensor_combined_s raw;
@@ -199,7 +199,7 @@ l_sensor_combined(struct listener *l)
}
void
-l_vehicle_attitude(struct listener *l)
+l_vehicle_attitude(const struct listener *l)
{
struct vehicle_attitude_s att;
@@ -222,7 +222,7 @@ l_vehicle_attitude(struct listener *l)
}
void
-l_vehicle_gps_position(struct listener *l)
+l_vehicle_gps_position(const struct listener *l)
{
struct vehicle_gps_position_s gps;
@@ -256,7 +256,7 @@ l_vehicle_gps_position(struct listener *l)
}
void
-l_vehicle_status(struct listener *l)
+l_vehicle_status(const struct listener *l)
{
/* immediately communicate state changes back to user */
orb_copy(ORB_ID(vehicle_status), status_sub, &v_status);
@@ -280,7 +280,7 @@ l_vehicle_status(struct listener *l)
}
void
-l_rc_channels(struct listener *l)
+l_rc_channels(const struct listener *l)
{
/* copy rc channels into local buffer */
orb_copy(ORB_ID(rc_channels), rc_sub, &rc);
@@ -288,7 +288,7 @@ l_rc_channels(struct listener *l)
}
void
-l_input_rc(struct listener *l)
+l_input_rc(const struct listener *l)
{
/* copy rc channels into local buffer */
orb_copy(ORB_ID(input_rc), mavlink_subs.input_rc_sub, &rc_raw);
@@ -310,7 +310,7 @@ l_input_rc(struct listener *l)
}
void
-l_global_position(struct listener *l)
+l_global_position(const struct listener *l)
{
/* copy global position data into local buffer */
orb_copy(ORB_ID(vehicle_global_position), mavlink_subs.global_pos_sub, &global_pos);
@@ -340,7 +340,7 @@ l_global_position(struct listener *l)
}
void
-l_local_position(struct listener *l)
+l_local_position(const struct listener *l)
{
/* copy local position data into local buffer */
orb_copy(ORB_ID(vehicle_local_position), mavlink_subs.local_pos_sub, &local_pos);
@@ -357,7 +357,7 @@ l_local_position(struct listener *l)
}
void
-l_global_position_setpoint(struct listener *l)
+l_global_position_setpoint(const struct listener *l)
{
struct vehicle_global_position_setpoint_s global_sp;
@@ -379,7 +379,7 @@ l_global_position_setpoint(struct listener *l)
}
void
-l_local_position_setpoint(struct listener *l)
+l_local_position_setpoint(const struct listener *l)
{
struct vehicle_local_position_setpoint_s local_sp;
@@ -396,7 +396,7 @@ l_local_position_setpoint(struct listener *l)
}
void
-l_attitude_setpoint(struct listener *l)
+l_attitude_setpoint(const struct listener *l)
{
struct vehicle_attitude_setpoint_s att_sp;
@@ -413,7 +413,7 @@ l_attitude_setpoint(struct listener *l)
}
void
-l_vehicle_rates_setpoint(struct listener *l)
+l_vehicle_rates_setpoint(const struct listener *l)
{
struct vehicle_rates_setpoint_s rates_sp;
@@ -430,7 +430,7 @@ l_vehicle_rates_setpoint(struct listener *l)
}
void
-l_actuator_outputs(struct listener *l)
+l_actuator_outputs(const struct listener *l)
{
struct actuator_outputs_s act_outputs;
@@ -529,13 +529,13 @@ l_actuator_outputs(struct listener *l)
}
void
-l_actuator_armed(struct listener *l)
+l_actuator_armed(const struct listener *l)
{
orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed);
}
void
-l_manual_control_setpoint(struct listener *l)
+l_manual_control_setpoint(const struct listener *l)
{
struct manual_control_setpoint_s man_control;
@@ -553,7 +553,7 @@ l_manual_control_setpoint(struct listener *l)
}
void
-l_vehicle_attitude_controls(struct listener *l)
+l_vehicle_attitude_controls(const struct listener *l)
{
struct actuator_controls_effective_s actuators;
@@ -581,7 +581,7 @@ l_vehicle_attitude_controls(struct listener *l)
}
void
-l_debug_key_value(struct listener *l)
+l_debug_key_value(const struct listener *l)
{
struct debug_key_value_s debug;
@@ -597,7 +597,7 @@ l_debug_key_value(struct listener *l)
}
void
-l_optical_flow(struct listener *l)
+l_optical_flow(const struct listener *l)
{
struct optical_flow_s flow;
@@ -608,7 +608,7 @@ l_optical_flow(struct listener *l)
}
void
-l_home(struct listener *l)
+l_home(const struct listener *l)
{
struct home_position_s home;
diff --git a/apps/px4/tests/test_bson.c b/apps/px4/tests/test_bson.c
index a42c462ca..6130fe763 100644
--- a/apps/px4/tests/test_bson.c
+++ b/apps/px4/tests/test_bson.c
@@ -51,7 +51,7 @@ static const int32_t sample_small_int = 123;
static const int64_t sample_big_int = (int64_t)INT_MAX + 123LL;
static const double sample_double = 2.5f;
static const char *sample_string = "this is a test";
-static const uint8_t sample_data[256];
+static const uint8_t sample_data[256] = {0};
//static const char *sample_filename = "/fs/microsd/bson.test";
static int
diff --git a/apps/systemlib/param/param.c b/apps/systemlib/param/param.c
index ebb72ca3e..8073570d1 100644
--- a/apps/systemlib/param/param.c
+++ b/apps/systemlib/param/param.c
@@ -87,7 +87,7 @@ struct param_wbuf_s {
UT_array *param_values;
/** array info for the modified parameters array */
-UT_icd param_icd = {sizeof(struct param_wbuf_s), NULL, NULL, NULL};
+const UT_icd param_icd = {sizeof(struct param_wbuf_s), NULL, NULL, NULL};
/** parameter update topic */
ORB_DEFINE(parameter_update, struct parameter_update_s);