diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-03-16 14:58:59 -0700 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-03-16 14:58:59 -0700 |
commit | 1b16387a996dcad53c3179b053162f7fee543254 (patch) | |
tree | 5c3b5a06cc04a12aad4472b7190b8d09fd6e81bf /apps | |
parent | 6b947a67d07eadc7dc882edf4505377085979784 (diff) | |
parent | de078b4525c3a7f34fc3955fb9fef3564f20d62a (diff) | |
download | px4-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-x | apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c | 66 | ||||
-rw-r--r-- | apps/drivers/blinkm/blinkm.cpp | 8 | ||||
-rw-r--r-- | apps/mavlink/orb_listener.c | 82 | ||||
-rw-r--r-- | apps/px4/tests/test_bson.c | 2 | ||||
-rw-r--r-- | apps/systemlib/param/param.c | 2 |
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); |