From d9767eb100ad6965012dfb945992843d157782d9 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 7 Nov 2013 22:23:57 +0400 Subject: [PATCH 01/54] Battery current reading implemented. --- src/modules/sensors/sensor_params.c | 2 ++ src/modules/sensors/sensors.cpp | 40 +++++++++++++++++++++-------- 2 files changed, 31 insertions(+), 11 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 587b815886..09ddbf5032 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -197,12 +197,14 @@ PARAM_DEFINE_INT32(RC_DSM_BIND, -1); /* -1 = Idle, 0 = Start DSM2 bind, 1 = Star #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f); +PARAM_DEFINE_FLOAT(BAT_C_SCALING, 0.001f); // TODO set correct default value #else /* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */ /* PX4IOAR: 0.00838095238 */ /* FMU standalone: 1/(10 / (47+10)) * (3.3 / 4095) = 0.00459340659 */ /* FMU with PX4IOAR: (3.3f * 52.0f / 5.0f / 4095.0f) */ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.00459340659f); +PARAM_DEFINE_FLOAT(BAT_C_SCALING, 0.001f); // TODO set correct default value #endif PARAM_DEFINE_INT32(RC_MAP_ROLL, 1); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index da0c11372c..789a4a30de 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -261,6 +261,7 @@ private: float rc_scale_flaps; float battery_voltage_scaling; + float battery_current_scaling; } _parameters; /**< local copies of interesting parameters */ @@ -306,6 +307,7 @@ private: param_t rc_scale_flaps; param_t battery_voltage_scaling; + param_t battery_current_scaling; param_t board_rotation; param_t external_mag_rotation; @@ -547,6 +549,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"); @@ -724,6 +727,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)); @@ -1162,25 +1170,25 @@ Sensors::adc_poll(struct sensor_combined_s &raw) float voltage = (buf_adc[i].am_data * _parameters.battery_voltage_scaling); if (voltage > VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) { - /* one-time initialization of low-pass value to avoid long init delays */ - if (_battery_status.voltage_v < 3.0f) { + if (_battery_status.voltage_v < VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) { _battery_status.voltage_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; - /* 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.timestamp = 0; + } - } 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.timestamp != 0) { + _battery_status.current_a = (buf_adc[i].am_data * _parameters.battery_current_scaling); + float dt = fminf(20.0f, (hrt_absolute_time() - _last_adc) * 0.001f); // in ms, limit to 20ms + _battery_status.discharged_mah += _battery_status.current_a * dt; } } else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) { @@ -1214,6 +1222,16 @@ Sensors::adc_poll(struct sensor_combined_s &raw) _last_adc = hrt_absolute_time(); } } + + if (_battery_status.timestamp != 0) { + /* announce the battery status if needed, just publish else */ + if (_battery_pub > 0) { + orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status); + + } else { + _battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status); + } + } } } From 08b2c338f605d4d9ffa15b151368e127ead241e3 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 7 Nov 2013 22:38:24 +0400 Subject: [PATCH 02/54] Workaround to compile on FMUv1. --- src/modules/sensors/sensors.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 789a4a30de..313923bcb7 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -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 From 697df775f91ad279cb92d220a4a941f464f0628a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 8 Nov 2013 21:28:22 +0400 Subject: [PATCH 03/54] sensors: fixed bug discharged battery current --- src/modules/sensors/sensors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 313923bcb7..2cd122ce5c 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1189,7 +1189,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw) if (_battery_status.timestamp != 0) { _battery_status.current_a = (buf_adc[i].am_data * _parameters.battery_current_scaling); float dt = fminf(20.0f, (hrt_absolute_time() - _last_adc) * 0.001f); // in ms, limit to 20ms - _battery_status.discharged_mah += _battery_status.current_a * dt; + _battery_status.discharged_mah += _battery_status.current_a * dt / 3600.0f; } } else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) { From 1a318ee2a60d95f7d64fe7ed13db8e5377b8c98c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 8 Nov 2013 21:29:26 +0400 Subject: [PATCH 04/54] sdlog2: log all low-level battery parameters in BATT message --- src/modules/sdlog2/sdlog2.c | 22 +++++++++++++++++++--- src/modules/sdlog2/sdlog2_messages.h | 13 ++++++++++--- 2 files changed, 29 insertions(+), 6 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index f94875d5b8..a9c0ec6e10 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -704,6 +704,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)); @@ -729,6 +730,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 */ @@ -755,6 +757,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) @@ -764,7 +767,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ /* number of messages */ - const ssize_t fdsc = 20; + const ssize_t fdsc = 21; /* Sanity check variable and index */ ssize_t fdsc_count = 0; /* file descriptors to wait for */ @@ -890,6 +893,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. @@ -980,8 +989,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; @@ -1252,6 +1259,15 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(GVSP); } + /* --- BATTERY --- */ + if (fds[ifds++].revents & POLLIN) { + log_msg.msg_type = LOG_BATT_MSG; + log_msg.body.log_BATT.voltage = buf.battery.voltage_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 */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 90093a407c..c4c176722f 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -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; @@ -247,6 +245,14 @@ struct log_GVSP_s { float vz; }; +/* --- BATT - BATTERY --- */ +#define LOG_BATT_MSG 20 +struct log_BATT_s { + float voltage; + float current; + float discharged; +}; + /* --- TIME - TIME STAMP --- */ #define LOG_TIME_MSG 129 struct log_TIME_s { @@ -280,7 +286,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, "BBBBB", "MainState,NavState,ArmState,BatRem,BatWarn,Landed"), LOG_FORMAT(RC, "ffffffff", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"), @@ -290,6 +296,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, "fff", "V,C,Discharged"), /* system-level messages, ID >= 0x80 */ // FMT: don't write format of format message, it's useless LOG_FORMAT(TIME, "Q", "StartTime"), From 6b085e8ced81d946307074f2ae44d6d62c63f170 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 8 Nov 2013 21:30:10 +0400 Subject: [PATCH 05/54] Use discharged current to estimate remaining battery charge if capacity is known --- src/modules/commander/commander.cpp | 3 +- src/modules/commander/commander_helper.cpp | 40 +++++++++++++--------- src/modules/commander/commander_helper.h | 7 ++-- src/modules/commander/commander_params.c | 3 +- 4 files changed, 32 insertions(+), 21 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ace13bb784..2499aff082 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -896,8 +896,9 @@ int commander_thread_main(int argc, char *argv[]) /* 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; + 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_v, battery.discharged_mah); } } diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 565b4b66ab..49fe5ea4d4 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -251,36 +251,44 @@ 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)); + if (bat_capacity > 0.0f) { + /* if battery capacity is known, use it to estimate remaining charge */ + ret = 1.0f - discharged / bat_capacity; + } else { + /* else use voltage */ + ret = (voltage - bat_n_cells * bat_v_empty) / (bat_n_cells * (bat_v_full - bat_v_empty)); + } /* limit to sane values */ ret = (ret < 0.0f) ? 0.0f : ret; diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index e9514446c1..d0393f45a9 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -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_ */ diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 40d0386d5e..bdb4a0a1c7 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -52,4 +52,5 @@ 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_INT32(BAT_N_CELLS, 3); +PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f); From e2f50f7bf880023a397957da5615d356f30f2ae4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 9 Nov 2013 17:56:40 +0400 Subject: [PATCH 06/54] Fix mavlink battery remaining scale --- src/modules/mavlink/mavlink.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 7c10e297b9..5713db5120 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -678,7 +678,7 @@ int mavlink_thread_main(int argc, char *argv[]) v_status.load * 1000.0f, v_status.battery_voltage * 1000.0f, v_status.battery_current * 1000.0f, - v_status.battery_remaining, + v_status.battery_remaining * 100.0f, v_status.drop_rate_comm, v_status.errors_comm, v_status.errors_count1, From 2761ea4adcc18ba326dbf24490a2f41fb0f8b9f1 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 9 Nov 2013 23:29:45 +0400 Subject: [PATCH 07/54] sdlog2: BATT message format fixed --- src/modules/sdlog2/sdlog2_messages.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index c4c176722f..95045134fa 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -286,7 +286,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, "BBBBB", "MainState,NavState,ArmState,BatRem,BatWarn,Landed"), + LOG_FORMAT(STAT, "BBBfBB", "MainState,NavState,ArmState,BatRem,BatWarn,Landed"), LOG_FORMAT(RC, "ffffffff", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"), From 75c57010d622df1bce39b27d3691337e5b11871e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 10 Nov 2013 00:06:00 +0400 Subject: [PATCH 08/54] sdlog2: BATT message bug fixed --- src/modules/sdlog2/sdlog2.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index a9c0ec6e10..8ab4c34ef2 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1261,6 +1261,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- 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.current = buf.battery.current_a; From 20db1602d73b318dfc12f71fbef94a52e9073073 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 10 Nov 2013 00:12:40 +0400 Subject: [PATCH 09/54] mavlink battery current scale fix --- src/modules/mavlink/mavlink.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 5713db5120..20853379de 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -677,7 +677,7 @@ 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_current * 100.0f, v_status.battery_remaining * 100.0f, v_status.drop_rate_comm, v_status.errors_comm, From e8487b7498e8a47dd93915f7ace10d97618a6969 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 10 Nov 2013 15:51:47 +0400 Subject: [PATCH 10/54] sensors: minor cleanup, bugs fixed, use unsigned long for discharged integration to avoid rounding errors. --- src/modules/sensors/sensors.cpp | 54 +++++++++++++----------- src/modules/uORB/topics/battery_status.h | 6 +-- 2 files changed, 33 insertions(+), 27 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 2cd122ce5c..d6231ac69a 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -126,8 +126,7 @@ #endif #define BAT_VOL_INITIAL 0.f -#define BAT_VOL_LOWPASS_1 0.99f -#define BAT_VOL_LOWPASS_2 0.01f +#define BAT_VOL_LOWPASS 0.01f #define VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS 3.5f /** @@ -216,6 +215,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 */ + unsigned long _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]; @@ -462,7 +464,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 */ @@ -1149,17 +1153,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); @@ -1176,8 +1179,8 @@ Sensors::adc_poll(struct sensor_combined_s &raw) _battery_status.voltage_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));; + _battery_status.timestamp = t; + _battery_status.voltage_v += (voltage - _battery_status.voltage_v) * BAT_VOL_LOWPASS; } else { /* mark status as invalid */ @@ -1187,9 +1190,14 @@ Sensors::adc_poll(struct sensor_combined_s &raw) } else if (ADC_BATTERY_CURRENT_CHANNEL == buf_adc[i].am_channel) { /* handle current only if voltage is valid */ if (_battery_status.timestamp != 0) { - _battery_status.current_a = (buf_adc[i].am_data * _parameters.battery_current_scaling); - float dt = fminf(20.0f, (hrt_absolute_time() - _last_adc) * 0.001f); // in ms, limit to 20ms - _battery_status.discharged_mah += _battery_status.current_a * dt / 3600.0f; + float current = (buf_adc[i].am_data * _parameters.battery_current_scaling); + _battery_status.timestamp = t; + _battery_status.current_a = current; + if (_battery_current_timestamp != 0) { + _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) { @@ -1206,7 +1214,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; @@ -1219,18 +1227,16 @@ Sensors::adc_poll(struct sensor_combined_s &raw) } } } - - _last_adc = hrt_absolute_time(); } - } + _last_adc = t; + if (_battery_status.timestamp != 0) { + /* announce the battery status if needed, just publish else */ + if (_battery_pub > 0) { + orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status); - if (_battery_status.timestamp != 0) { - /* announce the battery status if needed, just publish else */ - if (_battery_pub > 0) { - orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status); - - } else { - _battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status); + } else { + _battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status); + } } } } diff --git a/src/modules/uORB/topics/battery_status.h b/src/modules/uORB/topics/battery_status.h index c40d0d4e56..62796c62c3 100644 --- a/src/modules/uORB/topics/battery_status.h +++ b/src/modules/uORB/topics/battery_status.h @@ -54,8 +54,8 @@ 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 current_a; /**< Battery current in amperes, -1 if unknown */ + float discharged_mah; /**< Discharged amount in mAh, -1 if unknown */ }; /** @@ -65,4 +65,4 @@ struct battery_status_s { /* register this as object request broker structure */ ORB_DECLARE(battery_status); -#endif \ No newline at end of file +#endif From 714f5ea634a184ac80254e2a415221f738d2ecd6 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 11 Nov 2013 22:02:55 +0400 Subject: [PATCH 11/54] Track raw battery voltage and filtered battery voltage separately. Estimate remaining battery as min(voltage_estimate, discharged_estimate). Battery voltage LPF time increased. --- src/modules/commander/commander.cpp | 64 ++++++---------------- src/modules/commander/commander_helper.cpp | 10 +++- src/modules/sdlog2/sdlog2.c | 1 + src/modules/sdlog2/sdlog2_messages.h | 3 +- src/modules/sensors/sensors.cpp | 44 +++++++++------ src/modules/uORB/topics/battery_status.h | 3 +- 6 files changed, 57 insertions(+), 68 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2499aff082..e6eaa742bd 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -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,15 +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(battery.voltage_v, battery.discharged_mah); + status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah); } } @@ -951,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 */ diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 49fe5ea4d4..21a1c4c2c7 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -44,6 +44,7 @@ #include #include #include +#include #include #include @@ -282,12 +283,15 @@ float battery_remaining_estimate_voltage(float voltage, float discharged) 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)); + if (bat_capacity > 0.0f) { - /* if battery capacity is known, use it to estimate remaining charge */ - ret = 1.0f - discharged / bat_capacity; + /* 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 = (voltage - bat_n_cells * bat_v_empty) / (bat_n_cells * (bat_v_full - bat_v_empty)); + ret = remaining_voltage; } /* limit to sane values */ diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 8ab4c34ef2..72cfb4d0d0 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1264,6 +1264,7 @@ int sdlog2_thread_main(int argc, char *argv[]) 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); diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 95045134fa..b02f398588 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -249,6 +249,7 @@ struct log_GVSP_s { #define LOG_BATT_MSG 20 struct log_BATT_s { float voltage; + float voltage_filtered; float current; float discharged; }; @@ -296,7 +297,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, "fff", "V,C,Discharged"), + 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"), diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index d6231ac69a..c23f295526 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -125,9 +125,8 @@ #define ADC_AIRSPEED_VOLTAGE_CHANNEL 15 #endif -#define BAT_VOL_INITIAL 0.f -#define BAT_VOL_LOWPASS 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, @@ -1173,32 +1172,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 < VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) { - _battery_status.voltage_v = voltage; + if (_battery_status.voltage_filtered_v < BATT_V_IGNORE_THRESHOLD) { + _battery_status.voltage_filtered_v = voltage; } _battery_status.timestamp = t; - _battery_status.voltage_v += (voltage - _battery_status.voltage_v) * BAT_VOL_LOWPASS; + _battery_status.voltage_filtered_v += (voltage - _battery_status.voltage_filtered_v) * BATT_V_LOWPASS; } else { /* mark status as invalid */ - _battery_status.timestamp = 0; + _battery_status.voltage_v = -1.0f; + _battery_status.voltage_filtered_v = -1.0f; } } else if (ADC_BATTERY_CURRENT_CHANNEL == buf_adc[i].am_channel) { /* handle current only if voltage is valid */ - if (_battery_status.timestamp != 0) { + if (_battery_status.voltage_v > 0.0f) { float current = (buf_adc[i].am_data * _parameters.battery_current_scaling); - _battery_status.timestamp = t; - _battery_status.current_a = current; - if (_battery_current_timestamp != 0) { - _battery_discharged += current * (t - _battery_current_timestamp); - _battery_status.discharged_mah = ((float) _battery_discharged) / 3600000.0f; + /* 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; } + _battery_current_timestamp = t; } else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) { @@ -1229,7 +1236,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw) } } _last_adc = t; - if (_battery_status.timestamp != 0) { + 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); @@ -1512,7 +1519,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); diff --git a/src/modules/uORB/topics/battery_status.h b/src/modules/uORB/topics/battery_status.h index 62796c62c3..d473dff3f4 100644 --- a/src/modules/uORB/topics/battery_status.h +++ b/src/modules/uORB/topics/battery_status.h @@ -53,7 +53,8 @@ */ struct battery_status_s { uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ - float voltage_v; /**< Battery voltage in volts, filtered */ + 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 */ }; From 2a2c8337e8a01c59a542c8dd3dc77a087b34e3c2 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 25 Nov 2013 19:22:06 +0400 Subject: [PATCH 12/54] sensors: discharged current type changed to uint64 --- src/modules/sensors/sensors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 8ac2114afd..f205ff8f54 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -214,7 +214,7 @@ 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 */ - unsigned long _battery_discharged; /**< battery discharged current in mA*ms */ + uint64_t _battery_discharged; /**< battery discharged current in mA*ms */ hrt_abstime _battery_current_timestamp; /**< timestamp of last battery current reading */ struct { From 7f14f1f7deb945b7f0ba14c2f49758e9a79d12a3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 31 Dec 2013 14:45:38 +0100 Subject: [PATCH 13/54] Add conversions and mixer tests. Work in progress --- Tools/tests-host/Makefile | 10 +- Tools/tests-host/hrt.cpp | 13 ++ Tools/tests-host/mixer_test.cpp | 2 + Tools/tests-host/queue.h | 133 ++++++++++++++++++++ src/modules/systemlib/pwm_limit/pwm_limit.c | 30 ++--- src/modules/systemlib/pwm_limit/pwm_limit.h | 18 +-- src/systemcmds/tests/module.mk | 3 +- src/systemcmds/tests/test_conv.cpp | 76 +++++++++++ src/systemcmds/tests/test_mixer.cpp | 52 +++++++- src/systemcmds/tests/test_rc.c | 4 +- src/systemcmds/tests/tests.h | 1 + src/systemcmds/tests/tests_main.c | 1 + 12 files changed, 315 insertions(+), 28 deletions(-) create mode 100644 Tools/tests-host/hrt.cpp create mode 100644 Tools/tests-host/queue.h create mode 100644 src/systemcmds/tests/test_conv.cpp diff --git a/Tools/tests-host/Makefile b/Tools/tests-host/Makefile index 97410ff47c..7ab1454f01 100644 --- a/Tools/tests-host/Makefile +++ b/Tools/tests-host/Makefile @@ -10,11 +10,13 @@ LIBS=-lm #_DEPS = test.h #DEPS = $(patsubst %,$(IDIR)/%,$(_DEPS)) -_OBJ = mixer_test.o test_mixer.o mixer_simple.o mixer_multirotor.o mixer.o mixer_group.o mixer_load.o +_OBJ = mixer_test.o test_mixer.o mixer_simple.o mixer_multirotor.o \ + mixer.o mixer_group.o mixer_load.o test_conv.o pwm_limit.o hrt.o OBJ = $(patsubst %,$(ODIR)/%,$(_OBJ)) #$(DEPS) $(ODIR)/%.o: %.cpp + mkdir -p obj $(CC) -c -o $@ $< $(CFLAGS) $(ODIR)/%.o: ../../src/systemcmds/tests/%.cpp @@ -26,6 +28,12 @@ $(ODIR)/%.o: ../../src/modules/systemlib/%.cpp $(ODIR)/%.o: ../../src/modules/systemlib/mixer/%.cpp $(CC) -c -o $@ $< $(CFLAGS) +$(ODIR)/%.o: ../../src/modules/systemlib/pwm_limit/%.cpp + $(CC) -c -o $@ $< $(CFLAGS) + +$(ODIR)/%.o: ../../src/modules/systemlib/pwm_limit/%.c + $(CC) -c -o $@ $< $(CFLAGS) + $(ODIR)/%.o: ../../src/modules/systemlib/mixer/%.c $(CC) -c -o $@ $< $(CFLAGS) diff --git a/Tools/tests-host/hrt.cpp b/Tools/tests-host/hrt.cpp new file mode 100644 index 0000000000..dc9fa23dea --- /dev/null +++ b/Tools/tests-host/hrt.cpp @@ -0,0 +1,13 @@ +#include +#include +#include +#include + +uint64_t hrt_absolute_time() +{ + struct timeval te; + gettimeofday(&te, NULL); // get current time + unsigned long long us = static_cast(te.tv_sec) * 1e6 + te.tv_usec; // caculate us + printf("us: %lld\n", us); + return us; +} \ No newline at end of file diff --git a/Tools/tests-host/mixer_test.cpp b/Tools/tests-host/mixer_test.cpp index 042322aadc..e311617f96 100644 --- a/Tools/tests-host/mixer_test.cpp +++ b/Tools/tests-host/mixer_test.cpp @@ -9,4 +9,6 @@ int main(int argc, char *argv[]) { "../../ROMFS/px4fmu_common/mixers/FMU_quad_w.mix"}; test_mixer(3, args); + + test_conv(1, args); } \ No newline at end of file diff --git a/Tools/tests-host/queue.h b/Tools/tests-host/queue.h new file mode 100644 index 0000000000..0fdb170db6 --- /dev/null +++ b/Tools/tests-host/queue.h @@ -0,0 +1,133 @@ +/************************************************************************ + * include/queue.h + * + * Copyright (C) 2007-2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ************************************************************************/ + +#ifndef __INCLUDE_QUEUE_H +#define __INCLUDE_QUEUE_H + +#ifndef FAR +#define FAR +#endif + +/************************************************************************ + * Included Files + ************************************************************************/ + +#include + +/************************************************************************ + * Pre-processor Definitions + ************************************************************************/ + +#define sq_init(q) do { (q)->head = NULL; (q)->tail = NULL; } while (0) +#define dq_init(q) do { (q)->head = NULL; (q)->tail = NULL; } while (0) + +#define sq_next(p) ((p)->flink) +#define dq_next(p) ((p)->flink) +#define dq_prev(p) ((p)->blink) + +#define sq_empty(q) ((q)->head == NULL) +#define dq_empty(q) ((q)->head == NULL) + +#define sq_peek(q) ((q)->head) +#define dq_peek(q) ((q)->head) + +/************************************************************************ + * Global Type Declarations + ************************************************************************/ + +struct sq_entry_s +{ + FAR struct sq_entry_s *flink; +}; +typedef struct sq_entry_s sq_entry_t; + +struct dq_entry_s +{ + FAR struct dq_entry_s *flink; + FAR struct dq_entry_s *blink; +}; +typedef struct dq_entry_s dq_entry_t; + +struct sq_queue_s +{ + FAR sq_entry_t *head; + FAR sq_entry_t *tail; +}; +typedef struct sq_queue_s sq_queue_t; + +struct dq_queue_s +{ + FAR dq_entry_t *head; + FAR dq_entry_t *tail; +}; +typedef struct dq_queue_s dq_queue_t; + +/************************************************************************ + * Global Function Prototypes + ************************************************************************/ + +#ifdef __cplusplus +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + +EXTERN void sq_addfirst(FAR sq_entry_t *node, sq_queue_t *queue); +EXTERN void dq_addfirst(FAR dq_entry_t *node, dq_queue_t *queue); +EXTERN void sq_addlast(FAR sq_entry_t *node, sq_queue_t *queue); +EXTERN void dq_addlast(FAR dq_entry_t *node, dq_queue_t *queue); +EXTERN void sq_addafter(FAR sq_entry_t *prev, FAR sq_entry_t *node, + sq_queue_t *queue); +EXTERN void dq_addafter(FAR dq_entry_t *prev, FAR dq_entry_t *node, + dq_queue_t *queue); +EXTERN void dq_addbefore(FAR dq_entry_t *next, FAR dq_entry_t *node, + dq_queue_t *queue); + +EXTERN FAR sq_entry_t *sq_remafter(FAR sq_entry_t *node, sq_queue_t *queue); +EXTERN void sq_rem(FAR sq_entry_t *node, sq_queue_t *queue); +EXTERN void dq_rem(FAR dq_entry_t *node, dq_queue_t *queue); +EXTERN FAR sq_entry_t *sq_remlast(sq_queue_t *queue); +EXTERN FAR dq_entry_t *dq_remlast(dq_queue_t *queue); +EXTERN FAR sq_entry_t *sq_remfirst(sq_queue_t *queue); +EXTERN FAR dq_entry_t *dq_remfirst(dq_queue_t *queue); + +#undef EXTERN +#ifdef __cplusplus +} +#endif + +#endif /* __INCLUDE_QUEUE_H_ */ + diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c index cac3dc82a3..992a7024b1 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.c +++ b/src/modules/systemlib/pwm_limit/pwm_limit.c @@ -47,7 +47,7 @@ void pwm_limit_init(pwm_limit_t *limit) { - limit->state = LIMIT_STATE_OFF; + limit->state = PWM_LIMIT_STATE_OFF; limit->time_armed = 0; return; } @@ -56,26 +56,26 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_ { /* first evaluate state changes */ switch (limit->state) { - case LIMIT_STATE_OFF: + case PWM_LIMIT_STATE_OFF: if (armed) - limit->state = LIMIT_STATE_RAMP; + limit->state = PWM_LIMIT_STATE_RAMP; limit->time_armed = hrt_absolute_time(); break; - case LIMIT_STATE_INIT: + case PWM_LIMIT_STATE_INIT: if (!armed) - limit->state = LIMIT_STATE_OFF; + limit->state = PWM_LIMIT_STATE_OFF; else if (hrt_absolute_time() - limit->time_armed >= INIT_TIME_US) - limit->state = LIMIT_STATE_RAMP; + limit->state = PWM_LIMIT_STATE_RAMP; break; - case LIMIT_STATE_RAMP: + case PWM_LIMIT_STATE_RAMP: if (!armed) - limit->state = LIMIT_STATE_OFF; + limit->state = PWM_LIMIT_STATE_OFF; else if (hrt_absolute_time() - limit->time_armed >= INIT_TIME_US + RAMP_TIME_US) - limit->state = LIMIT_STATE_ON; + limit->state = PWM_LIMIT_STATE_ON; break; - case LIMIT_STATE_ON: + case PWM_LIMIT_STATE_ON: if (!armed) - limit->state = LIMIT_STATE_OFF; + limit->state = PWM_LIMIT_STATE_OFF; break; default: break; @@ -86,14 +86,14 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_ /* then set effective_pwm based on state */ switch (limit->state) { - case LIMIT_STATE_OFF: - case LIMIT_STATE_INIT: + case PWM_LIMIT_STATE_OFF: + case PWM_LIMIT_STATE_INIT: for (unsigned i=0; itime_armed)*10000 / RAMP_TIME_US; for (unsigned i=0; i #include +__BEGIN_DECLS + /* * time for the ESCs to initialize * (this is not actually needed if PWM is sent right after boot) @@ -56,18 +58,18 @@ */ #define RAMP_TIME_US 2500000 +enum pwm_limit_state { + PWM_LIMIT_STATE_OFF = 0, + PWM_LIMIT_STATE_INIT, + PWM_LIMIT_STATE_RAMP, + PWM_LIMIT_STATE_ON +}; + typedef struct { - enum { - LIMIT_STATE_OFF = 0, - LIMIT_STATE_INIT, - LIMIT_STATE_RAMP, - LIMIT_STATE_ON - } state; + enum pwm_limit_state state; uint64_t time_armed; } pwm_limit_t; -__BEGIN_DECLS - __EXPORT void pwm_limit_init(pwm_limit_t *limit); __EXPORT void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, float *output, uint16_t *effective_pwm, pwm_limit_t *limit); diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk index 68a080c77c..576caff994 100644 --- a/src/systemcmds/tests/module.mk +++ b/src/systemcmds/tests/module.mk @@ -28,4 +28,5 @@ SRCS = test_adc.c \ tests_main.c \ test_param.c \ test_ppm_loopback.c \ - test_rc.c + test_rc.c \ + test_conv.cpp diff --git a/src/systemcmds/tests/test_conv.cpp b/src/systemcmds/tests/test_conv.cpp new file mode 100644 index 0000000000..50dece816b --- /dev/null +++ b/src/systemcmds/tests/test_conv.cpp @@ -0,0 +1,76 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file test_conv.cpp + * Tests conversions used across the system. + * + */ + +#include + +#include + +#include +#include +#include +#include +#include +#include + +#include "tests.h" + +#include +#include + +#include +#include +#include + +int test_conv(int argc, char *argv[]) +{ + warnx("Testing system conversions"); + + for (int i = -10000; i <= 10000; i+=1) { + float f = i/10000.0f; + float fres = REG_TO_FLOAT(FLOAT_TO_REG(f)); + if (fabsf(f - fres) > 0.0001f) { + warnx("conversion fail: input: %8.4f, intermediate: %d, result: %8.4f", f, REG_TO_SIGNED(FLOAT_TO_REG(f)), fres); + return 1; + } + } + + warnx("All conversions clean"); + + return 0; +} diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index 4da86042d2..d330da39f3 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -51,6 +51,7 @@ #include #include +#include #include "tests.h" @@ -59,6 +60,18 @@ static int mixer_callback(uintptr_t handle, uint8_t control_index, float &control); +const unsigned output_max = 8; +static float actuator_controls[output_max]; +static bool should_arm = false; +uint16_t r_page_servo_disarmed[output_max]; +uint16_t r_page_servo_control_min[output_max]; +uint16_t r_page_servo_control_max[output_max]; +uint16_t r_page_servos[output_max]; +/* + * PWM limit structure + */ +pwm_limit_t pwm_limit; + int test_mixer(int argc, char *argv[]) { warnx("testing mixer"); @@ -164,6 +177,40 @@ int test_mixer(int argc, char *argv[]) if (mixer_group.count() != 8) return 1; + /* execute the mixer */ + + float outputs_unlimited[output_max]; + float outputs[output_max]; + unsigned mixed; + const int jmax = 50; + + pwm_limit_init(&pwm_limit); + pwm_limit.state = PWM_LIMIT_STATE_ON; + should_arm = true; + + for (int j = -jmax; j <= jmax; j++) { + + for (int i = 0; i < output_max; i++) { + actuator_controls[i] = j/100.0f + 0.1f * i; + r_page_servo_disarmed[i] = 900; + r_page_servo_control_min[i] = 1000; + r_page_servo_control_max[i] = 2000; + } + + /* mix */ + mixed = mixer_group.mix(&outputs_unlimited[0], output_max); + + memcpy(outputs, outputs_unlimited, sizeof(outputs)); + + pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); + + warnx("mixed %d outputs (max %d), values:", mixed, output_max); + for (int i = 0; i < mixed; i++) + { + printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]); + } + } + /* load multirotor at once test */ mixer_group.reset(); @@ -193,7 +240,10 @@ mixer_callback(uintptr_t handle, if (control_group != 0) return -1; - control = 0.0f; + if (control_index > (sizeof(actuator_controls) / sizeof(actuator_controls[0]))) + return -1; + + control = actuator_controls[control_index]; return 0; } diff --git a/src/systemcmds/tests/test_rc.c b/src/systemcmds/tests/test_rc.c index 72619fc8ba..6a602ecfc9 100644 --- a/src/systemcmds/tests/test_rc.c +++ b/src/systemcmds/tests/test_rc.c @@ -32,8 +32,8 @@ ****************************************************************************/ /** - * @file test_ppm_loopback.c - * Tests the PWM outputs and PPM input + * @file test_rc.c + * Tests RC input. * */ diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h index a57d04be37..321e91e604 100644 --- a/src/systemcmds/tests/tests.h +++ b/src/systemcmds/tests/tests.h @@ -109,6 +109,7 @@ extern int test_bson(int argc, char *argv[]); extern int test_file(int argc, char *argv[]); extern int test_mixer(int argc, char *argv[]); extern int test_rc(int argc, char *argv[]); +extern int test_conv(int argc, char *argv[]); __END_DECLS diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index 1088a44076..1c717d3eaf 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -106,6 +106,7 @@ const struct { {"file", test_file, 0}, {"mixer", test_mixer, OPT_NOJIGTEST | OPT_NOALLTEST}, {"rc", test_rc, OPT_NOJIGTEST | OPT_NOALLTEST}, + {"conv", test_conv, OPT_NOJIGTEST | OPT_NOALLTEST}, {"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST}, {NULL, NULL, 0} }; From eb47a164cb44ddab70a2639ddf4f68519eadd4df Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 2 Jan 2014 01:18:12 +0100 Subject: [PATCH 14/54] Added missing HRT functionality, cleanup --- Tools/tests-host/hrt.cpp | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/Tools/tests-host/hrt.cpp b/Tools/tests-host/hrt.cpp index dc9fa23dea..d791e9e2a1 100644 --- a/Tools/tests-host/hrt.cpp +++ b/Tools/tests-host/hrt.cpp @@ -3,11 +3,14 @@ #include #include -uint64_t hrt_absolute_time() -{ +hrt_abstime hrt_absolute_time() { struct timeval te; gettimeofday(&te, NULL); // get current time - unsigned long long us = static_cast(te.tv_sec) * 1e6 + te.tv_usec; // caculate us - printf("us: %lld\n", us); + hrt_abstime us = static_cast(te.tv_sec) * 1e6 + te.tv_usec; // caculate us return us; +} + +hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then) { + // not thread safe + return hrt_absolute_time() - *then; } \ No newline at end of file From a60fcc25358ac9ef142b456874459bec160bcbb1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 2 Jan 2014 09:18:04 +0100 Subject: [PATCH 15/54] Fixed pwm limit command to behave as originally designed. The initial hold time produced random values (e.g. 40000 instead of 1500) during the INIT_TIME (0.5s) phase --- src/modules/systemlib/pwm_limit/pwm_limit.c | 90 ++++++++++++--------- src/modules/systemlib/pwm_limit/pwm_limit.h | 2 +- 2 files changed, 55 insertions(+), 37 deletions(-) diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c index 992a7024b1..190b315f1f 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.c +++ b/src/modules/systemlib/pwm_limit/pwm_limit.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. * Author: Julian Oes * * Redistribution and use in source and binary forms, with or without @@ -44,38 +44,53 @@ #include #include #include +#include void pwm_limit_init(pwm_limit_t *limit) { - limit->state = PWM_LIMIT_STATE_OFF; + limit->state = PWM_LIMIT_STATE_INIT; limit->time_armed = 0; return; } -void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, float *output, uint16_t *effective_pwm, pwm_limit_t *limit) +void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, const float *output, uint16_t *effective_pwm, pwm_limit_t *limit) { + /* first evaluate state changes */ switch (limit->state) { - case PWM_LIMIT_STATE_OFF: - if (armed) - limit->state = PWM_LIMIT_STATE_RAMP; - limit->time_armed = hrt_absolute_time(); - break; case PWM_LIMIT_STATE_INIT: - if (!armed) - limit->state = PWM_LIMIT_STATE_OFF; - else if (hrt_absolute_time() - limit->time_armed >= INIT_TIME_US) + + if (armed) { + + /* set arming time for the first call */ + if (limit->time_armed == 0) { + limit->time_armed = hrt_absolute_time(); + } + + if (hrt_elapsed_time(&limit->time_armed) >= INIT_TIME_US) { + limit->state = PWM_LIMIT_STATE_OFF; + } + } + break; + case PWM_LIMIT_STATE_OFF: + if (armed) { limit->state = PWM_LIMIT_STATE_RAMP; + + /* reset arming time, used for ramp timing */ + limit->time_armed = hrt_absolute_time(); + } break; case PWM_LIMIT_STATE_RAMP: - if (!armed) + if (!armed) { limit->state = PWM_LIMIT_STATE_OFF; - else if (hrt_absolute_time() - limit->time_armed >= INIT_TIME_US + RAMP_TIME_US) + } else if (hrt_elapsed_time(&limit->time_armed) >= RAMP_TIME_US) { limit->state = PWM_LIMIT_STATE_ON; + } break; case PWM_LIMIT_STATE_ON: - if (!armed) + if (!armed) { limit->state = PWM_LIMIT_STATE_OFF; + } break; default: break; @@ -90,40 +105,43 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_ case PWM_LIMIT_STATE_INIT: for (unsigned i=0; itime_armed); - progress = (hrt_absolute_time() - INIT_TIME_US - limit->time_armed)*10000 / RAMP_TIME_US; - for (unsigned i=0; i 0) { + progress = diff * 10000 / RAMP_TIME_US; - /* safeguard against overflows */ - uint16_t disarmed = disarmed_pwm[i]; - if (disarmed > min_pwm[i]) - disarmed = min_pwm[i]; + for (unsigned i=0; i 0) { - uint16_t disarmed_min_diff = min_pwm[i] - disarmed; - ramp_min_pwm = disarmed + (disarmed_min_diff * progress) / 10000; - } else { - - /* no disarmed pwm value set, choose min pwm */ - ramp_min_pwm = min_pwm[i]; + /* safeguard against overflows */ + unsigned disarmed = disarmed_pwm[i]; + if (disarmed > min_pwm[i]) { + disarmed = min_pwm[i]; + } + + unsigned disarmed_min_diff = min_pwm[i] - disarmed; + ramp_min_pwm = disarmed + (disarmed_min_diff * progress) / 10000; + + } else { + + /* no disarmed pwm value set, choose min pwm */ + ramp_min_pwm = min_pwm[i]; + } + + effective_pwm[i] = output[i] * (max_pwm[i] - ramp_min_pwm)/2 + (max_pwm[i] + ramp_min_pwm)/2; } - - effective_pwm[i] = output[i] * (max_pwm[i] - ramp_min_pwm)/2 + (max_pwm[i] + ramp_min_pwm)/2; - output[i] = (float)progress/10000.0f * output[i]; } break; case PWM_LIMIT_STATE_ON: for (unsigned i=0; i Date: Thu, 2 Jan 2014 09:18:36 +0100 Subject: [PATCH 16/54] Turned the mixer test into a real test, now also cross checking post mix results --- src/systemcmds/tests/test_mixer.cpp | 80 +++++++++++++++++++++++++---- 1 file changed, 70 insertions(+), 10 deletions(-) diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index d330da39f3..460e6f5e7d 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -48,10 +48,12 @@ #include #include #include +#include #include #include #include +#include #include "tests.h" @@ -67,6 +69,8 @@ uint16_t r_page_servo_disarmed[output_max]; uint16_t r_page_servo_control_min[output_max]; uint16_t r_page_servo_control_max[output_max]; uint16_t r_page_servos[output_max]; +uint16_t servo_predicted[output_max]; + /* * PWM limit structure */ @@ -179,35 +183,87 @@ int test_mixer(int argc, char *argv[]) /* execute the mixer */ - float outputs_unlimited[output_max]; float outputs[output_max]; unsigned mixed; - const int jmax = 50; + const int jmax = 5; pwm_limit_init(&pwm_limit); - pwm_limit.state = PWM_LIMIT_STATE_ON; should_arm = true; + /* run through arming phase */ + for (int i = 0; i < output_max; i++) { + actuator_controls[i] = 0.1f; + r_page_servo_disarmed[i] = 900; + r_page_servo_control_min[i] = 1000; + r_page_servo_control_max[i] = 2000; + } + + warnx("ARMING TEST: STARTING RAMP"); + unsigned sleep_quantum_us = 10000; + + hrt_abstime starttime = hrt_absolute_time(); + unsigned sleepcount = 0; + + while (hrt_elapsed_time(&starttime) < INIT_TIME_US + RAMP_TIME_US) { + + /* mix */ + mixed = mixer_group.mix(&outputs[0], output_max); + + pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); + + //warnx("mixed %d outputs (max %d), values:", mixed, output_max); + for (int i = 0; i < mixed; i++) + { + /* check mixed outputs to be zero during init phase */ + if (hrt_elapsed_time(&starttime) < INIT_TIME_US && + r_page_servos[i] != r_page_servo_disarmed[i]) { + warnx("disarmed servo value mismatch"); + return 1; + } + + if (hrt_elapsed_time(&starttime) >= INIT_TIME_US && + r_page_servos[i] + 1 <= r_page_servo_disarmed[i]) { + warnx("ramp servo value mismatch"); + return 1; + } + + //printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]); + } + usleep(sleep_quantum_us); + sleepcount++; + + if (sleepcount % 10 == 0) { + printf("."); + fflush(stdout); + } + } + printf("\n"); + + warnx("ARMING TEST: NORMAL OPERATION"); + for (int j = -jmax; j <= jmax; j++) { for (int i = 0; i < output_max; i++) { - actuator_controls[i] = j/100.0f + 0.1f * i; + actuator_controls[i] = j/10.0f + 0.1f * i; r_page_servo_disarmed[i] = 900; r_page_servo_control_min[i] = 1000; r_page_servo_control_max[i] = 2000; } /* mix */ - mixed = mixer_group.mix(&outputs_unlimited[0], output_max); - - memcpy(outputs, outputs_unlimited, sizeof(outputs)); + mixed = mixer_group.mix(&outputs[0], output_max); pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); - warnx("mixed %d outputs (max %d), values:", mixed, output_max); + warnx("mixed %d outputs (max %d)", mixed, output_max); for (int i = 0; i < mixed; i++) { - printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]); + servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f; + if (fabsf(servo_predicted[i] - r_page_servos[i]) > 2) { + printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, outputs[i], servo_predicted[i], (int)r_page_servos[i]); + warnx("mixer violated predicted value"); + return 1; + } } } @@ -227,8 +283,12 @@ int test_mixer(int argc, char *argv[]) unsigned mc_loaded = loaded; mixer_group.load_from_buf(&buf[0], mc_loaded); warnx("complete buffer load: loaded %u mixers", mixer_group.count()); - if (mixer_group.count() != 8) + if (mixer_group.count() != 5) { + warnx("FAIL: Quad W mixer load failed"); return 1; + } + + warnx("SUCCESS: No errors in mixer test"); } static int From 7ae71316fa90a2da250411022a14d8e9432a8e35 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 2 Jan 2014 09:49:43 +0100 Subject: [PATCH 17/54] Missing line break at EOF --- Tools/tests-host/hrt.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/tests-host/hrt.cpp b/Tools/tests-host/hrt.cpp index d791e9e2a1..01b5958b7c 100644 --- a/Tools/tests-host/hrt.cpp +++ b/Tools/tests-host/hrt.cpp @@ -13,4 +13,4 @@ hrt_abstime hrt_absolute_time() { hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then) { // not thread safe return hrt_absolute_time() - *then; -} \ No newline at end of file +} From 9612514a3f59cfedbd7b29c9e4f30c1edf1c7345 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 2 Jan 2014 09:50:09 +0100 Subject: [PATCH 18/54] Testing disarming and rearming as well now, removed magic numbers in favor of constants --- src/systemcmds/tests/test_mixer.cpp | 95 +++++++++++++++++++++++++++-- 1 file changed, 89 insertions(+), 6 deletions(-) diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index 460e6f5e7d..2a47551ee6 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -54,6 +54,7 @@ #include #include #include +#include #include "tests.h" @@ -193,9 +194,9 @@ int test_mixer(int argc, char *argv[]) /* run through arming phase */ for (int i = 0; i < output_max; i++) { actuator_controls[i] = 0.1f; - r_page_servo_disarmed[i] = 900; - r_page_servo_control_min[i] = 1000; - r_page_servo_control_max[i] = 2000; + r_page_servo_disarmed[i] = PWM_LOWEST_MIN; + r_page_servo_control_min[i] = PWM_DEFAULT_MIN; + r_page_servo_control_max[i] = PWM_DEFAULT_MAX; } warnx("ARMING TEST: STARTING RAMP"); @@ -245,9 +246,9 @@ int test_mixer(int argc, char *argv[]) for (int i = 0; i < output_max; i++) { actuator_controls[i] = j/10.0f + 0.1f * i; - r_page_servo_disarmed[i] = 900; - r_page_servo_control_min[i] = 1000; - r_page_servo_control_max[i] = 2000; + r_page_servo_disarmed[i] = PWM_LOWEST_MIN; + r_page_servo_control_min[i] = PWM_DEFAULT_MIN; + r_page_servo_control_max[i] = PWM_DEFAULT_MAX; } /* mix */ @@ -267,6 +268,88 @@ int test_mixer(int argc, char *argv[]) } } + warnx("ARMING TEST: DISARMING"); + + starttime = hrt_absolute_time(); + sleepcount = 0; + should_arm = false; + + while (hrt_elapsed_time(&starttime) < 600000) { + + /* mix */ + mixed = mixer_group.mix(&outputs[0], output_max); + + pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); + + //warnx("mixed %d outputs (max %d), values:", mixed, output_max); + for (int i = 0; i < mixed; i++) + { + /* check mixed outputs to be zero during init phase */ + if (r_page_servos[i] != r_page_servo_disarmed[i]) { + warnx("disarmed servo value mismatch"); + return 1; + } + + //printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]); + } + usleep(sleep_quantum_us); + sleepcount++; + + if (sleepcount % 10 == 0) { + printf("."); + fflush(stdout); + } + } + printf("\n"); + + warnx("ARMING TEST: REARMING: STARTING RAMP"); + + starttime = hrt_absolute_time(); + sleepcount = 0; + should_arm = true; + + while (hrt_elapsed_time(&starttime) < 600000 + RAMP_TIME_US) { + + /* mix */ + mixed = mixer_group.mix(&outputs[0], output_max); + + pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); + + //warnx("mixed %d outputs (max %d), values:", mixed, output_max); + for (int i = 0; i < mixed; i++) + { + /* predict value */ + servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f; + + /* check ramp */ + + if (hrt_elapsed_time(&starttime) < RAMP_TIME_US && + (r_page_servos[i] + 1 <= r_page_servo_disarmed[i] || + r_page_servos[i] > servo_predicted[i])) { + warnx("ramp servo value mismatch"); + return 1; + } + + /* check post ramp phase */ + if (hrt_elapsed_time(&starttime) > RAMP_TIME_US && + fabsf(servo_predicted[i] - r_page_servos[i]) > 2) { + printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, outputs[i], servo_predicted[i], (int)r_page_servos[i]); + warnx("mixer violated predicted value"); + return 1; + } + + //printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]); + } + usleep(sleep_quantum_us); + sleepcount++; + + if (sleepcount % 10 == 0) { + printf("."); + fflush(stdout); + } + } + printf("\n"); + /* load multirotor at once test */ mixer_group.reset(); From f205c07c084fd1008f201518d84c64718e7df9cc Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 10 Jan 2014 22:38:12 +0100 Subject: [PATCH 19/54] very simple gps fix fake in gps driver only for development --- src/drivers/gps/gps.cpp | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index fc500a9ecd..1afb279fe6 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -294,6 +294,29 @@ GPS::task_main() while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) { // lock(); /* opportunistic publishing - else invalid data would end up on the bus */ +//#define FAKEGPS +#ifdef FAKEGPS + _report.timestamp_position = hrt_absolute_time(); + _report.lat = (int32_t)47.378301e7f; + _report.lon = (int32_t)8.538777e7f; + _report.alt = (int32_t)400e3f; + _report.timestamp_variance = hrt_absolute_time(); + _report.s_variance_m_s = 10.0f; + _report.p_variance_m = 10.0f; + _report.c_variance_rad = 0.1f; + _report.fix_type = 3; + _report.eph_m = 10.0f; + _report.epv_m = 10.0f; + _report.timestamp_velocity = hrt_absolute_time(); + _report.vel_n_m_s = 20.0f; + _report.vel_e_m_s = 0.0f; + _report.vel_d_m_s = 0.0f; + _report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s); + _report.cog_rad = 0.0f; + _report.vel_ned_valid = true; + + //no time and satellite information simulated +#endif if (_report_pub > 0) { orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); From a522c64fee57c8e5e0cd188e589c8bcf87b5396d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 11 Jan 2014 00:08:02 +0100 Subject: [PATCH 20/54] fake gps: gps device is not needed for fake position generation --- src/drivers/gps/gps.cpp | 57 ++++++++++++++++++++++++----------------- 1 file changed, 34 insertions(+), 23 deletions(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 1afb279fe6..3649a41d6c 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -264,6 +264,39 @@ GPS::task_main() /* loop handling received serial bytes and also configuring in between */ while (!_task_should_exit) { +//#define FAKEGPS +#ifdef FAKEGPS + _report.timestamp_position = hrt_absolute_time(); + _report.lat = (int32_t)47.378301e7f; + _report.lon = (int32_t)8.538777e7f; + _report.alt = (int32_t)400e3f; + _report.timestamp_variance = hrt_absolute_time(); + _report.s_variance_m_s = 10.0f; + _report.p_variance_m = 10.0f; + _report.c_variance_rad = 0.1f; + _report.fix_type = 3; + _report.eph_m = 10.0f; + _report.epv_m = 10.0f; + _report.timestamp_velocity = hrt_absolute_time(); + _report.vel_n_m_s = 20.0f; + _report.vel_e_m_s = 0.0f; + _report.vel_d_m_s = 0.0f; + _report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s); + _report.cog_rad = 0.0f; + _report.vel_ned_valid = true; + + //no time and satellite information simulated + + if (_report_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + + } else { + _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + } + + usleep(2e5); +#else + if (_Helper != nullptr) { delete(_Helper); /* set to zero to ensure parser is not used while not instantiated */ @@ -294,29 +327,6 @@ GPS::task_main() while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) { // lock(); /* opportunistic publishing - else invalid data would end up on the bus */ -//#define FAKEGPS -#ifdef FAKEGPS - _report.timestamp_position = hrt_absolute_time(); - _report.lat = (int32_t)47.378301e7f; - _report.lon = (int32_t)8.538777e7f; - _report.alt = (int32_t)400e3f; - _report.timestamp_variance = hrt_absolute_time(); - _report.s_variance_m_s = 10.0f; - _report.p_variance_m = 10.0f; - _report.c_variance_rad = 0.1f; - _report.fix_type = 3; - _report.eph_m = 10.0f; - _report.epv_m = 10.0f; - _report.timestamp_velocity = hrt_absolute_time(); - _report.vel_n_m_s = 20.0f; - _report.vel_e_m_s = 0.0f; - _report.vel_d_m_s = 0.0f; - _report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s); - _report.cog_rad = 0.0f; - _report.vel_ned_valid = true; - - //no time and satellite information simulated -#endif if (_report_pub > 0) { orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); @@ -380,6 +390,7 @@ GPS::task_main() default: break; } +#endif } From 2ed315480e4582c9f223b88e1fee39303fbc9248 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 11 Jan 2014 00:19:43 +0100 Subject: [PATCH 21/54] fakegps: default to 0 m/s speed --- src/drivers/gps/gps.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 3649a41d6c..7df9cdb6d6 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -264,7 +264,7 @@ GPS::task_main() /* loop handling received serial bytes and also configuring in between */ while (!_task_should_exit) { -//#define FAKEGPS +#define FAKEGPS #ifdef FAKEGPS _report.timestamp_position = hrt_absolute_time(); _report.lat = (int32_t)47.378301e7f; @@ -278,7 +278,7 @@ GPS::task_main() _report.eph_m = 10.0f; _report.epv_m = 10.0f; _report.timestamp_velocity = hrt_absolute_time(); - _report.vel_n_m_s = 20.0f; + _report.vel_n_m_s = 0.0f; _report.vel_e_m_s = 0.0f; _report.vel_d_m_s = 0.0f; _report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s); From bccd0f8947f8d95f048b421b06fde25d162aac50 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 11 Jan 2014 00:46:45 +0100 Subject: [PATCH 22/54] fakegps: add command-line option --- src/drivers/gps/gps.cpp | 260 +++++++++++++++++++++------------------- 1 file changed, 135 insertions(+), 125 deletions(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 7df9cdb6d6..6b72d560fa 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -85,7 +85,7 @@ static const int ERROR = -1; class GPS : public device::CDev { public: - GPS(const char *uart_path); + GPS(const char *uart_path, bool fake_gps); virtual ~GPS(); virtual int init(); @@ -112,6 +112,7 @@ private: struct vehicle_gps_position_s _report; ///< uORB topic for gps position orb_advert_t _report_pub; ///< uORB pub for gps position float _rate; ///< position update rate + bool _fake_gps; ///< fake gps output /** @@ -156,7 +157,7 @@ GPS *g_dev; } -GPS::GPS(const char *uart_path) : +GPS::GPS(const char *uart_path, bool fake_gps) : CDev("gps", GPS_DEVICE_PATH), _task_should_exit(false), _healthy(false), @@ -164,7 +165,8 @@ GPS::GPS(const char *uart_path) : _mode(GPS_DRIVER_MODE_UBX), _Helper(nullptr), _report_pub(-1), - _rate(0.0f) + _rate(0.0f), + _fake_gps(fake_gps) { /* store port name */ strncpy(_port, uart_path, sizeof(_port)); @@ -264,134 +266,135 @@ GPS::task_main() /* loop handling received serial bytes and also configuring in between */ while (!_task_should_exit) { -#define FAKEGPS -#ifdef FAKEGPS - _report.timestamp_position = hrt_absolute_time(); - _report.lat = (int32_t)47.378301e7f; - _report.lon = (int32_t)8.538777e7f; - _report.alt = (int32_t)400e3f; - _report.timestamp_variance = hrt_absolute_time(); - _report.s_variance_m_s = 10.0f; - _report.p_variance_m = 10.0f; - _report.c_variance_rad = 0.1f; - _report.fix_type = 3; - _report.eph_m = 10.0f; - _report.epv_m = 10.0f; - _report.timestamp_velocity = hrt_absolute_time(); - _report.vel_n_m_s = 0.0f; - _report.vel_e_m_s = 0.0f; - _report.vel_d_m_s = 0.0f; - _report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s); - _report.cog_rad = 0.0f; - _report.vel_ned_valid = true; + if (_fake_gps) { - //no time and satellite information simulated + _report.timestamp_position = hrt_absolute_time(); + _report.lat = (int32_t)47.378301e7f; + _report.lon = (int32_t)8.538777e7f; + _report.alt = (int32_t)400e3f; + _report.timestamp_variance = hrt_absolute_time(); + _report.s_variance_m_s = 10.0f; + _report.p_variance_m = 10.0f; + _report.c_variance_rad = 0.1f; + _report.fix_type = 3; + _report.eph_m = 10.0f; + _report.epv_m = 10.0f; + _report.timestamp_velocity = hrt_absolute_time(); + _report.vel_n_m_s = 0.0f; + _report.vel_e_m_s = 0.0f; + _report.vel_d_m_s = 0.0f; + _report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s); + _report.cog_rad = 0.0f; + _report.vel_ned_valid = true; - if (_report_pub > 0) { - orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + //no time and satellite information simulated - } else { - _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); - } + if (_report_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); - usleep(2e5); -#else - - if (_Helper != nullptr) { - delete(_Helper); - /* set to zero to ensure parser is not used while not instantiated */ - _Helper = nullptr; - } - - switch (_mode) { - case GPS_DRIVER_MODE_UBX: - _Helper = new UBX(_serial_fd, &_report); - break; - - case GPS_DRIVER_MODE_MTK: - _Helper = new MTK(_serial_fd, &_report); - break; - - default: - break; - } - - unlock(); - - if (_Helper->configure(_baudrate) == 0) { - unlock(); - - // GPS is obviously detected successfully, reset statistics - _Helper->reset_update_rates(); - - while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) { -// lock(); - /* opportunistic publishing - else invalid data would end up on the bus */ - if (_report_pub > 0) { - orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); - - } else { - _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); - } - - last_rate_count++; - - /* measure update rate every 5 seconds */ - if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) { - _rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f); - last_rate_measurement = hrt_absolute_time(); - last_rate_count = 0; - _Helper->store_update_rates(); - _Helper->reset_update_rates(); - } - - if (!_healthy) { - char *mode_str = "unknown"; - - switch (_mode) { - case GPS_DRIVER_MODE_UBX: - mode_str = "UBX"; - break; - - case GPS_DRIVER_MODE_MTK: - mode_str = "MTK"; - break; - - default: - break; - } - - warnx("module found: %s", mode_str); - _healthy = true; - } + } else { + _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); } - if (_healthy) { - warnx("module lost"); - _healthy = false; - _rate = 0.0f; + usleep(2e5); + + } else { + + if (_Helper != nullptr) { + delete(_Helper); + /* set to zero to ensure parser is not used while not instantiated */ + _Helper = nullptr; + } + + switch (_mode) { + case GPS_DRIVER_MODE_UBX: + _Helper = new UBX(_serial_fd, &_report); + break; + + case GPS_DRIVER_MODE_MTK: + _Helper = new MTK(_serial_fd, &_report); + break; + + default: + break; + } + + unlock(); + + if (_Helper->configure(_baudrate) == 0) { + unlock(); + + // GPS is obviously detected successfully, reset statistics + _Helper->reset_update_rates(); + + while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) { + // lock(); + /* opportunistic publishing - else invalid data would end up on the bus */ + if (_report_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + + } else { + _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + } + + last_rate_count++; + + /* measure update rate every 5 seconds */ + if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) { + _rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f); + last_rate_measurement = hrt_absolute_time(); + last_rate_count = 0; + _Helper->store_update_rates(); + _Helper->reset_update_rates(); + } + + if (!_healthy) { + char *mode_str = "unknown"; + + switch (_mode) { + case GPS_DRIVER_MODE_UBX: + mode_str = "UBX"; + break; + + case GPS_DRIVER_MODE_MTK: + mode_str = "MTK"; + break; + + default: + break; + } + + warnx("module found: %s", mode_str); + _healthy = true; + } + } + + if (_healthy) { + warnx("module lost"); + _healthy = false; + _rate = 0.0f; + } + + lock(); } lock(); + + /* select next mode */ + switch (_mode) { + case GPS_DRIVER_MODE_UBX: + _mode = GPS_DRIVER_MODE_MTK; + break; + + case GPS_DRIVER_MODE_MTK: + _mode = GPS_DRIVER_MODE_UBX; + break; + + default: + break; + } } - lock(); - - /* select next mode */ - switch (_mode) { - case GPS_DRIVER_MODE_UBX: - _mode = GPS_DRIVER_MODE_MTK; - break; - - case GPS_DRIVER_MODE_MTK: - _mode = GPS_DRIVER_MODE_UBX; - break; - - default: - break; - } -#endif - } warnx("exiting"); @@ -451,7 +454,7 @@ namespace gps GPS *g_dev; -void start(const char *uart_path); +void start(const char *uart_path, bool fake_gps); void stop(); void test(); void reset(); @@ -461,7 +464,7 @@ void info(); * Start the driver. */ void -start(const char *uart_path) +start(const char *uart_path, bool fake_gps) { int fd; @@ -469,7 +472,7 @@ start(const char *uart_path) errx(1, "already started"); /* create the driver */ - g_dev = new GPS(uart_path); + g_dev = new GPS(uart_path, fake_gps); if (g_dev == nullptr) goto fail; @@ -561,6 +564,7 @@ gps_main(int argc, char *argv[]) /* set to default */ char *device_name = GPS_DEFAULT_UART_PORT; + bool fake_gps = false; /* * Start/load the driver. @@ -576,7 +580,13 @@ gps_main(int argc, char *argv[]) } } - gps::start(device_name); + /* Detect fake gps option */ + for (int i = 2; i < argc; i++) { + if (!strcmp(argv[i], "-f")) + fake_gps = true; + } + + gps::start(device_name, fake_gps); } if (!strcmp(argv[1], "stop")) @@ -601,5 +611,5 @@ gps_main(int argc, char *argv[]) gps::info(); out: - errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n]"); + errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n][-f]"); } From 90e86bd4db36116a9241acbf28587956cf369c73 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 11 Jan 2014 20:52:44 +0100 Subject: [PATCH 23/54] Added support for byte MTD --- nuttx-configs/px4fmu-v2/nsh/defconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index c0ed11a621..ad845e095b 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -501,7 +501,7 @@ CONFIG_MTD=y # MTD Configuration # # CONFIG_MTD_PARTITION is not set -# CONFIG_MTD_BYTE_WRITE is not set +CONFIG_MTD_BYTE_WRITE=y # # MTD Device Drivers From 7e3802297c11e68279d092409f826013882177ef Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 11 Jan 2014 20:56:28 +0100 Subject: [PATCH 24/54] Added MTD adapter driver --- src/systemcmds/mtd/module.mk | 6 + src/systemcmds/mtd/mtd.c | 216 +++++++++++++++++++++++++++++++++++ 2 files changed, 222 insertions(+) create mode 100644 src/systemcmds/mtd/module.mk create mode 100644 src/systemcmds/mtd/mtd.c diff --git a/src/systemcmds/mtd/module.mk b/src/systemcmds/mtd/module.mk new file mode 100644 index 0000000000..6866565977 --- /dev/null +++ b/src/systemcmds/mtd/module.mk @@ -0,0 +1,6 @@ +# +# RAMTRON file system driver +# + +MODULE_COMMAND = mtd +SRCS = mtd.c diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c new file mode 100644 index 0000000000..43e42c9517 --- /dev/null +++ b/src/systemcmds/mtd/mtd.c @@ -0,0 +1,216 @@ +/**************************************************************************** + * + * 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mtd.c + * + * mtd service and utility app. + * + * @author Lorenz Meier + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include "systemlib/systemlib.h" +#include "systemlib/param/param.h" +#include "systemlib/err.h" + +__EXPORT int mtd_main(int argc, char *argv[]); + +#ifndef CONFIG_MTD_RAMTRON + +/* create a fake command with decent message to not confuse users */ +int mtd_main(int argc, char *argv[]) +{ + errx(1, "RAMTRON not enabled, skipping."); +} + +#else + +static void mtd_attach(void); +static void mtd_start(void); +static void mtd_erase(void); +static void mtd_ioctl(unsigned operation); +static void mtd_save(const char *name); +static void mtd_load(const char *name); +static void mtd_test(void); + +static bool attached = false; +static bool started = false; +static struct mtd_dev_s *mtd_dev; +static char *_mtdname = "/dev/mtd_params"; +static char *_wpname = "/dev/mtd_waypoints"; + +int mtd_main(int argc, char *argv[]) +{ + if (argc >= 2) { + if (!strcmp(argv[1], "start")) + mtd_start(); + + if (!strcmp(argv[1], "test")) + mtd_test(); + } + + errx(1, "expected a command, try 'start' or 'test'"); +} + +struct mtd_dev_s *ramtron_initialize(FAR struct spi_dev_s *dev); + + +static void +mtd_attach(void) +{ + /* find the right spi */ + struct spi_dev_s *spi = up_spiinitialize(2); + /* this resets the spi bus, set correct bus speed again */ + SPI_SETFREQUENCY(spi, 40 * 1000 * 1000); + SPI_SETBITS(spi, 8); + SPI_SETMODE(spi, SPIDEV_MODE3); + SPI_SELECT(spi, SPIDEV_FLASH, false); + + if (spi == NULL) + errx(1, "failed to locate spi bus"); + + /* start the MTD driver, attempt 5 times */ + for (int i = 0; i < 5; i++) { + mtd_dev = ramtron_initialize(spi); + + if (mtd_dev) { + /* abort on first valid result */ + if (i > 0) { + warnx("warning: mtd needed %d attempts to attach", i + 1); + } + + break; + } + } + + /* if last attempt is still unsuccessful, abort */ + if (mtd_dev == NULL) + errx(1, "failed to initialize mtd driver"); + + attached = true; +} + +static void +mtd_start(void) +{ + int ret; + + if (started) + errx(1, "mtd already mounted"); + + if (!attached) + mtd_attach(); + + if (!mtd_dev) { + warnx("ERROR: Failed to create RAMTRON FRAM MTD instance\n"); + exit(1); + } + + /* Initialize to provide an FTL block driver on the MTD FLASH interface. + * + * NOTE: We could just skip all of this FTL and BCH stuff. We could + * instead just use the MTD drivers bwrite and bread to perform this + * test. Creating the character drivers, however, makes this test more + * interesting. + */ + + ret = ftl_initialize(0, mtd_dev); + + if (ret < 0) { + warnx("Creating /dev/mtdblock0 failed: %d\n", ret); + exit(2); + } + + /* Now create a character device on the block device */ + + ret = bchdev_register("/dev/mtdblock0", _mtdname, false); + + if (ret < 0) { + warnx("ERROR: bchdev_register %s failed: %d\n", _mtdname, ret); + exit(3); + } + + /* mount the mtd */ + ret = mount(NULL, "/mtd", "nxffs", 0, NULL); + + if (ret < 0) + errx(1, "failed to mount /mtd - erase mtd to reformat"); + + started = true; + warnx("mounted mtd at /mtd"); + exit(0); +} + +static void +mtd_ioctl(unsigned operation) +{ + int fd; + + fd = open("/mtd/.", 0); + + if (fd < 0) + err(1, "open /mtd"); + + if (ioctl(fd, operation, 0) < 0) + err(1, "ioctl"); + + exit(0); +} + +static void +mtd_test(void) +{ +// at24c_test(); + exit(0); +} + +#endif From 6011ff9411ca64eb857d456a6ac2de2db07800c3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 11 Jan 2014 20:56:40 +0100 Subject: [PATCH 25/54] HIL cleanup --- src/drivers/hil/hil.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index c1d73dd87f..0a047f38f1 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -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; } From 4ea92eca7c492ec506c31fe59d0ba967052ccf27 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 11 Jan 2014 20:57:03 +0100 Subject: [PATCH 26/54] RGB led cleanup --- src/drivers/rgbled/rgbled.cpp | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index 727c86e02a..4f58891ed3 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -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 "); From 5a0c6353690a903b0cbf03aee7cb040726f25fd8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 11 Jan 2014 21:01:07 +0100 Subject: [PATCH 27/54] Added mtd tool --- makefiles/config_px4fmu-v2_default.mk | 1 + makefiles/config_px4fmu-v2_test.mk | 1 + 2 files changed, 2 insertions(+) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index afe072b644..99f26c3f39 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -60,6 +60,7 @@ MODULES += systemcmds/top MODULES += systemcmds/tests MODULES += systemcmds/config MODULES += systemcmds/nshterm +MODULES += systemcmds/mtd # # General system control diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index 6faef7e0a4..f54a4d8257 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -22,6 +22,7 @@ MODULES += systemcmds/perf MODULES += systemcmds/reboot MODULES += systemcmds/tests MODULES += systemcmds/nshterm +MODULES += systemcmds/mtd # # Library modules From 8205afc4e0ab8bd7b95abb21dee56f61feafc11e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 12 Jan 2014 13:11:01 +0100 Subject: [PATCH 28/54] Added missing file close on test command --- src/systemcmds/tests/test_file.c | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/src/systemcmds/tests/test_file.c b/src/systemcmds/tests/test_file.c index 7206b87d65..83d09dd5e8 100644 --- a/src/systemcmds/tests/test_file.c +++ b/src/systemcmds/tests/test_file.c @@ -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; } From 29e2c841bb9d91fa82e41906e4eb420604e9512f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 12 Jan 2014 15:34:05 +0100 Subject: [PATCH 29/54] Added support for N MTD / ramtron partitions / files --- src/systemcmds/mtd/mtd.c | 137 ++++++++++++++++++++++++--------------- 1 file changed, 85 insertions(+), 52 deletions(-) diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c index 43e42c9517..36ff7e262f 100644 --- a/src/systemcmds/mtd/mtd.c +++ b/src/systemcmds/mtd/mtd.c @@ -66,7 +66,7 @@ __EXPORT int mtd_main(int argc, char *argv[]); #ifndef CONFIG_MTD_RAMTRON -/* create a fake command with decent message to not confuse users */ +/* create a fake command with decent warnx to not confuse users */ int mtd_main(int argc, char *argv[]) { errx(1, "RAMTRON not enabled, skipping."); @@ -75,24 +75,30 @@ int mtd_main(int argc, char *argv[]) #else static void mtd_attach(void); -static void mtd_start(void); -static void mtd_erase(void); -static void mtd_ioctl(unsigned operation); -static void mtd_save(const char *name); -static void mtd_load(const char *name); +static void mtd_start(char *partition_names[], unsigned n_partitions); static void mtd_test(void); static bool attached = false; static bool started = false; static struct mtd_dev_s *mtd_dev; -static char *_mtdname = "/dev/mtd_params"; -static char *_wpname = "/dev/mtd_waypoints"; +static const int n_partitions_default = 2; + +/* note, these will be equally sized */ +static char *partition_names_default[n_partitions] = {"/dev/mtd_params", "/dev/mtd_waypoints"}; int mtd_main(int argc, char *argv[]) { if (argc >= 2) { - if (!strcmp(argv[1], "start")) - mtd_start(); + if (!strcmp(argv[1], "start")) { + + /* start mapping according to user request */ + if (argc > 3) { + mtd_start(argv + 2, argc - 2); + + } else { + mtd_start(partition_names_default, n_partitions_default); + } + } if (!strcmp(argv[1], "test")) mtd_test(); @@ -140,7 +146,7 @@ mtd_attach(void) } static void -mtd_start(void) +mtd_start(char *partition_names[], unsigned n_partitions) { int ret; @@ -155,62 +161,89 @@ mtd_start(void) exit(1); } - /* Initialize to provide an FTL block driver on the MTD FLASH interface. - * - * NOTE: We could just skip all of this FTL and BCH stuff. We could - * instead just use the MTD drivers bwrite and bread to perform this - * test. Creating the character drivers, however, makes this test more - * interesting. - */ - ret = ftl_initialize(0, mtd_dev); + /* Get the geometry of the FLASH device */ + + FAR struct mtd_geometry_s geo; + + ret = mtd_dev->ioctl(master, MTDIOC_GEOMETRY, (unsigned long)((uintptr_t)&geo)); if (ret < 0) { - warnx("Creating /dev/mtdblock0 failed: %d\n", ret); - exit(2); - } - - /* Now create a character device on the block device */ - - ret = bchdev_register("/dev/mtdblock0", _mtdname, false); - - if (ret < 0) { - warnx("ERROR: bchdev_register %s failed: %d\n", _mtdname, ret); + fdbg("ERROR: mtd->ioctl failed: %d\n", ret); exit(3); } - /* mount the mtd */ - ret = mount(NULL, "/mtd", "nxffs", 0, NULL); + warnx("Flash Geometry:\n"); + warnx(" blocksize: %lu\n", (unsigned long)geo.blocksize); + warnx(" erasesize: %lu\n", (unsigned long)geo.erasesize); + warnx(" neraseblocks: %lu\n", (unsigned long)geo.neraseblocks); - if (ret < 0) - errx(1, "failed to mount /mtd - erase mtd to reformat"); + /* Determine the size of each partition. Make each partition an even + * multiple of the erase block size (perhaps not using some space at the + * end of the FLASH). + */ + + unsigned blkpererase = geo.erasesize / geo.blocksize; + unsigned nblocks = (geo.neraseblocks / n_partitions) * blkpererase; + unsigned partsize = nblocks * geo.blocksize; + + warnx(" No. partitions: %u\n", n_partitions); + warnx(" Partition size: %lu Blocks (%lu bytes)\n", nblocks, partsize); + + /* Now create MTD FLASH partitions */ + + warnx("Creating partitions\n"); + FAR struct mtd_dev_s *part[n_partitions]; + char blockname[32]; + + for (unsigned offset = 0, unsigned i = 0; i < n_partitions; offset += nblocks, i++) { + + warnx(" Partition %d. Block offset=%lu, size=%lu\n", + i, (unsigned long)offset, (unsigned long)nblocks); + + /* Create the partition */ + + part[i] = mtd_partition(mtd_dev, offset, nblocks); + + if (!part[i]) { + warnx("ERROR: mtd_partition failed. offset=%lu nblocks=%lu\n", + (unsigned long)offset, (unsigned long)nblocks); + fsync(stderr); + exit(4); + } + + /* Initialize to provide an FTL block driver on the MTD FLASH interface */ + + snprintf(blockname, 32, "/dev/mtdblock%d", i); + + ret = ftl_initialize(i, part[i]); + + if (ret < 0) { + warnx("ERROR: ftl_initialize %s failed: %d\n", blockname, ret); + fsync(stderr); + exit(5); + } + + /* Now create a character device on the block device */ + + ret = bchdev_register(blockname, partition_names[i], false); + + if (ret < 0) { + warnx("ERROR: bchdev_register %s failed: %d\n", charname, ret); + fsync(stderr); + exit(6); + } + } started = true; - warnx("mounted mtd at /mtd"); - exit(0); -} - -static void -mtd_ioctl(unsigned operation) -{ - int fd; - - fd = open("/mtd/.", 0); - - if (fd < 0) - err(1, "open /mtd"); - - if (ioctl(fd, operation, 0) < 0) - err(1, "ioctl"); - exit(0); } static void mtd_test(void) { -// at24c_test(); - exit(0); + warnx("This test routine does not test anything yet!"); + exit(1); } #endif From 8b5adac0d94506927c8fc2efb39812b422e0e330 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 12 Jan 2014 15:53:06 +0100 Subject: [PATCH 30/54] Support for better param printing --- src/systemcmds/param/param.c | 33 ++++++++++++++++++++++++++++++--- 1 file changed, 30 insertions(+), 3 deletions(-) diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index 65f291f401..9e5e6f2e69 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -203,11 +203,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: ", From 3387aa64d40dc7be2900b0368293d9997b97005a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 12 Jan 2014 16:33:23 +0100 Subject: [PATCH 31/54] Enabled MTD partitions, successfully tested params --- ROMFS/px4fmu_common/init.d/rcS | 17 ++++++------ nuttx-configs/px4fmu-v1/nsh/defconfig | 21 +++++++++++++- nuttx-configs/px4fmu-v2/nsh/defconfig | 2 +- src/modules/systemlib/param/param.c | 27 ++++++++++++++---- src/systemcmds/mtd/mtd.c | 40 +++++++++++++++------------ 5 files changed, 74 insertions(+), 33 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 66cb3f2375..8de615746c 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -75,14 +75,15 @@ then # # Load microSD params # - #if ramtron start - #then - # param select /ramtron/params - # if [ -f /ramtron/params ] - # then - # param load /ramtron/params - # fi - #else + 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 diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index e60120b492..1dc96b3c3f 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -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 # diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index ad845e095b..2a734c27e5 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -500,7 +500,7 @@ CONFIG_MTD=y # # MTD Configuration # -# CONFIG_MTD_PARTITION is not set +CONFIG_MTD_PARTITION=y CONFIG_MTD_BYTE_WRITE=y # diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 398657dd79..b12ba2919f 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -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 @@ -512,6 +512,22 @@ param_save_default(void) int fd; const char *filename = param_get_default_file(); + + /* write parameters to temp file */ + fd = open(filename, O_WRONLY); + + if (fd < 0) { + warn("failed to open param file: %s", filename); + res = ERROR; + } + + if (res == OK) { + res = param_export(fd, false); + } + + close(fd); + +#if 0 const char *filename_tmp = malloc(strlen(filename) + 5); sprintf(filename_tmp, "%s.tmp", filename); @@ -563,6 +579,7 @@ param_save_default(void) } free(filename_tmp); +#endif return res; } @@ -573,9 +590,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 +601,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()); diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c index 36ff7e262f..5104df09e4 100644 --- a/src/systemcmds/mtd/mtd.c +++ b/src/systemcmds/mtd/mtd.c @@ -81,10 +81,10 @@ static void mtd_test(void); static bool attached = false; static bool started = false; static struct mtd_dev_s *mtd_dev; -static const int n_partitions_default = 2; /* note, these will be equally sized */ -static char *partition_names_default[n_partitions] = {"/dev/mtd_params", "/dev/mtd_waypoints"}; +static char *partition_names_default[] = {"/fs/mtd_params", "/fs/mtd_waypoints"}; +static const int n_partitions_default = sizeof(partition_names_default) / sizeof(partition_names_default[0]); int mtd_main(int argc, char *argv[]) { @@ -108,7 +108,8 @@ int mtd_main(int argc, char *argv[]) } struct mtd_dev_s *ramtron_initialize(FAR struct spi_dev_s *dev); - +struct mtd_dev_s *mtd_partition(FAR struct mtd_dev_s *mtd, + off_t firstblock, off_t nblocks); static void mtd_attach(void) @@ -157,7 +158,7 @@ mtd_start(char *partition_names[], unsigned n_partitions) mtd_attach(); if (!mtd_dev) { - warnx("ERROR: Failed to create RAMTRON FRAM MTD instance\n"); + warnx("ERROR: Failed to create RAMTRON FRAM MTD instance"); exit(1); } @@ -166,17 +167,17 @@ mtd_start(char *partition_names[], unsigned n_partitions) FAR struct mtd_geometry_s geo; - ret = mtd_dev->ioctl(master, MTDIOC_GEOMETRY, (unsigned long)((uintptr_t)&geo)); + ret = mtd_dev->ioctl(mtd_dev, MTDIOC_GEOMETRY, (unsigned long)((uintptr_t)&geo)); if (ret < 0) { - fdbg("ERROR: mtd->ioctl failed: %d\n", ret); + warnx("ERROR: mtd->ioctl failed: %d", ret); exit(3); } - warnx("Flash Geometry:\n"); - warnx(" blocksize: %lu\n", (unsigned long)geo.blocksize); - warnx(" erasesize: %lu\n", (unsigned long)geo.erasesize); - warnx(" neraseblocks: %lu\n", (unsigned long)geo.neraseblocks); + warnx("Flash Geometry:"); + warnx(" blocksize: %lu", (unsigned long)geo.blocksize); + warnx(" erasesize: %lu", (unsigned long)geo.erasesize); + warnx(" neraseblocks: %lu", (unsigned long)geo.neraseblocks); /* Determine the size of each partition. Make each partition an even * multiple of the erase block size (perhaps not using some space at the @@ -187,18 +188,21 @@ mtd_start(char *partition_names[], unsigned n_partitions) unsigned nblocks = (geo.neraseblocks / n_partitions) * blkpererase; unsigned partsize = nblocks * geo.blocksize; - warnx(" No. partitions: %u\n", n_partitions); - warnx(" Partition size: %lu Blocks (%lu bytes)\n", nblocks, partsize); + warnx(" No. partitions: %u", n_partitions); + warnx(" Partition size: %lu Blocks (%lu bytes)", nblocks, partsize); /* Now create MTD FLASH partitions */ - warnx("Creating partitions\n"); + warnx("Creating partitions"); FAR struct mtd_dev_s *part[n_partitions]; char blockname[32]; - for (unsigned offset = 0, unsigned i = 0; i < n_partitions; offset += nblocks, i++) { + unsigned offset; + unsigned i; - warnx(" Partition %d. Block offset=%lu, size=%lu\n", + for (offset = 0, i = 0; i < n_partitions; offset += nblocks, i++) { + + warnx(" Partition %d. Block offset=%lu, size=%lu", i, (unsigned long)offset, (unsigned long)nblocks); /* Create the partition */ @@ -206,7 +210,7 @@ mtd_start(char *partition_names[], unsigned n_partitions) part[i] = mtd_partition(mtd_dev, offset, nblocks); if (!part[i]) { - warnx("ERROR: mtd_partition failed. offset=%lu nblocks=%lu\n", + warnx("ERROR: mtd_partition failed. offset=%lu nblocks=%lu", (unsigned long)offset, (unsigned long)nblocks); fsync(stderr); exit(4); @@ -219,7 +223,7 @@ mtd_start(char *partition_names[], unsigned n_partitions) ret = ftl_initialize(i, part[i]); if (ret < 0) { - warnx("ERROR: ftl_initialize %s failed: %d\n", blockname, ret); + warnx("ERROR: ftl_initialize %s failed: %d", blockname, ret); fsync(stderr); exit(5); } @@ -229,7 +233,7 @@ mtd_start(char *partition_names[], unsigned n_partitions) ret = bchdev_register(blockname, partition_names[i], false); if (ret < 0) { - warnx("ERROR: bchdev_register %s failed: %d\n", charname, ret); + warnx("ERROR: bchdev_register %s failed: %d", partition_names[i], ret); fsync(stderr); exit(6); } From 16941714352f71789bb2a55e5116d7601d88749b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 12 Jan 2014 17:40:29 +0100 Subject: [PATCH 32/54] Compile / bugfixes on MTD commandline tool --- src/systemcmds/mtd/mtd.c | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c index 5104df09e4..c9a9ea97a9 100644 --- a/src/systemcmds/mtd/mtd.c +++ b/src/systemcmds/mtd/mtd.c @@ -189,7 +189,7 @@ mtd_start(char *partition_names[], unsigned n_partitions) unsigned partsize = nblocks * geo.blocksize; warnx(" No. partitions: %u", n_partitions); - warnx(" Partition size: %lu Blocks (%lu bytes)", nblocks, partsize); + warnx(" Partition size: %lu Blocks (%lu bytes)", (unsigned long)nblocks, (unsigned long)partsize); /* Now create MTD FLASH partitions */ @@ -212,7 +212,6 @@ mtd_start(char *partition_names[], unsigned n_partitions) if (!part[i]) { warnx("ERROR: mtd_partition failed. offset=%lu nblocks=%lu", (unsigned long)offset, (unsigned long)nblocks); - fsync(stderr); exit(4); } @@ -224,7 +223,6 @@ mtd_start(char *partition_names[], unsigned n_partitions) if (ret < 0) { warnx("ERROR: ftl_initialize %s failed: %d", blockname, ret); - fsync(stderr); exit(5); } @@ -234,7 +232,6 @@ mtd_start(char *partition_names[], unsigned n_partitions) if (ret < 0) { warnx("ERROR: bchdev_register %s failed: %d", partition_names[i], ret); - fsync(stderr); exit(6); } } From ab401da3e88c793ce2bbcaed34cffaf3f67eedd0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 12 Jan 2014 17:43:33 +0100 Subject: [PATCH 33/54] Fixed rcS typo --- ROMFS/px4fmu_common/init.d/rcS | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 8de615746c..ed034877f1 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -94,7 +94,7 @@ then echo "Parameter file corrupt - ignoring" fi fi - #fi + fi # # Start system state indicator From ea8ab2793a6683dbf7807c91e1a2c1d91187981e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 12 Jan 2014 18:52:10 +0100 Subject: [PATCH 34/54] More param command related improvements --- src/modules/systemlib/bson/tinybson.c | 3 +++ src/modules/systemlib/param/param.c | 14 ++++++++++---- src/systemcmds/param/param.c | 14 ++++++++------ 3 files changed, 21 insertions(+), 10 deletions(-) diff --git a/src/modules/systemlib/bson/tinybson.c b/src/modules/systemlib/bson/tinybson.c index 8aca6a25dd..49403c98bd 100644 --- a/src/modules/systemlib/bson/tinybson.c +++ b/src/modules/systemlib/bson/tinybson.c @@ -407,6 +407,9 @@ bson_encoder_fini(bson_encoder_t encoder) memcpy(encoder->buf, &len, sizeof(len)); } + /* sync file */ + fsync(encoder->fd); + return 0; } diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index b12ba2919f..2d773fd251 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -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) @@ -514,19 +514,25 @@ param_save_default(void) const char *filename = param_get_default_file(); /* write parameters to temp file */ - fd = open(filename, O_WRONLY); + fd = open(filename, O_WRONLY | O_CREAT); if (fd < 0) { warn("failed to open param file: %s", filename); - res = ERROR; + 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); @@ -579,9 +585,9 @@ param_save_default(void) } free(filename_tmp); -#endif return res; +#endif } /** diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index 9e5e6f2e69..580fdc62f4 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -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); } From f595b204eab82679a52ca5f43408797988cfdf42 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 13 Jan 2014 08:33:25 +0100 Subject: [PATCH 35/54] Parameter documentation improvements --- Tools/px4params/dokuwikiout.py | 30 ++- Tools/px4params/dokuwikiout_listings.py | 27 +++ .../fw_pos_control_l1_params.c | 67 ++++++- src/modules/sensors/module.mk | 2 +- src/modules/sensors/sensor_params.c | 186 ++++++++++++++++-- 5 files changed, 279 insertions(+), 33 deletions(-) create mode 100644 Tools/px4params/dokuwikiout_listings.py diff --git a/Tools/px4params/dokuwikiout.py b/Tools/px4params/dokuwikiout.py index 33f76b415e..4d40a6201d 100644 --- a/Tools/px4params/dokuwikiout.py +++ b/Tools/px4params/dokuwikiout.py @@ -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 diff --git a/Tools/px4params/dokuwikiout_listings.py b/Tools/px4params/dokuwikiout_listings.py new file mode 100644 index 0000000000..33f76b415e --- /dev/null +++ b/Tools/px4params/dokuwikiout_listings.py @@ -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 diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 3bb872405e..31a9cdefaf 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * 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); diff --git a/src/modules/sensors/module.mk b/src/modules/sensors/module.mk index ebbc580e10..aa538fd6b1 100644 --- a/src/modules/sensors/module.mk +++ b/src/modules/sensors/module.mk @@ -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 diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 7637235548..caec03e042 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -1,9 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * @author Thomas Gubler - * @author Julian Oes + * 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 @@ -38,6 +35,10 @@ * @file sensor_params.c * * Parameters defined by the sensors task. + * + * @author Lorenz Meier + * @author Julian Oes + * @author Thomas Gubler */ #include @@ -45,41 +46,98 @@ #include /** - * Gyro X offset FIXME + * Gyro X offset * - * This is an X-axis offset for the gyro. - * Adjust it according to the calibration data. + * This is an X-axis offset for the gyro. Adjust it according to the calibration data. * * @min -10.0 * @max 10.0 - * @group Gyro Config + * @group Sensor Calibration */ PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f); /** - * Gyro Y offset FIXME with dot. + * Gyro Y offset * * @min -10.0 * @max 10.0 - * @group Gyro Config + * @group Sensor Calibration */ PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f); /** - * Gyro Z offset FIXME + * Gyro Z offset * * @min -5.0 * @max 5.0 - * @group Gyro Config + * @group Sensor Calibration */ PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f); +/** + * Gyro X scaling + * + * X-axis scaling. + * + * @min -1.5 + * @max 1.5 + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f); + +/** + * Gyro Y scaling + * + * Y-axis scaling. + * + * @min -1.5 + * @max 1.5 + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_GYRO_YSCALE, 1.0f); + +/** + * Gyro Z scaling + * + * Z-axis scaling. + * + * @min -1.5 + * @max 1.5 + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_GYRO_ZSCALE, 1.0f); +/** + * Magnetometer X offset + * + * This is an X-axis offset for the magnetometer. + * + * @min -500.0 + * @max 500.0 + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_MAG_XOFF, 0.0f); + +/** + * Magnetometer Y offset + * + * This is an Y-axis offset for the magnetometer. + * + * @min -500.0 + * @max 500.0 + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_MAG_YOFF, 0.0f); + +/** + * Magnetometer Z offset + * + * This is an Z-axis offset for the magnetometer. + * + * @min -500.0 + * @max 500.0 + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_MAG_ZOFF, 0.0f); PARAM_DEFINE_FLOAT(SENS_MAG_XSCALE, 1.0f); @@ -100,16 +158,114 @@ PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0); PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0); +/** + * RC Channel 1 Minimum + * + * Minimum value for RC channel 1 + * + * @min 800.0 + * @max 1500.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f); + +/** + * RC Channel 1 Trim + * + * Mid point value (same as min for throttle) + * + * @min 800.0 + * @max 2200.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f); + +/** + * RC Channel 1 Maximum + * + * Maximum value for RC channel 1 + * + * @min 1500.0 + * @max 2200.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f); + +/** + * RC Channel 1 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC1_REV, 1.0f); + +/** + * RC Channel 1 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC1_DZ, 10.0f); -PARAM_DEFINE_FLOAT(RC2_MIN, 1000); -PARAM_DEFINE_FLOAT(RC2_TRIM, 1500); -PARAM_DEFINE_FLOAT(RC2_MAX, 2000); +/** + * RC Channel 2 Minimum + * + * Minimum value for RC channel 2 + * + * @min 800.0 + * @max 1500.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC2_MIN, 1000.0f); + +/** + * RC Channel 2 Trim + * + * Mid point value (same as min for throttle) + * + * @min 800.0 + * @max 2200.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC2_TRIM, 1500.0f); + +/** + * RC Channel 2 Maximum + * + * Maximum value for RC channel 2 + * + * @min 1500.0 + * @max 2200.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC2_MAX, 2000.0f); + +/** + * RC Channel 2 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC2_REV, 1.0f); + +/** + * RC Channel 2 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC2_DZ, 10.0f); PARAM_DEFINE_FLOAT(RC3_MIN, 1000); From db1ea9bf515afe927e7f1bde404210127ded0a4b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 13 Jan 2014 10:11:16 +0100 Subject: [PATCH 36/54] Battery: default parameters updated --- src/modules/commander/commander_params.c | 4 ++-- src/modules/sensors/sensor_params.c | 3 +-- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index bdb4a0a1c7..e10b7f18d5 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -50,7 +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_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); diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 3b3e56c880..bbc84ef938 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -372,15 +372,14 @@ PARAM_DEFINE_INT32(RC_DSM_BIND, -1); /* -1 = Idle, 0 = Start DSM2 bind, 1 = Star PARAM_DEFINE_INT32(BAT_V_SCALE_IO, 10000); #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f); -PARAM_DEFINE_FLOAT(BAT_C_SCALING, 0.001f); // TODO set correct default value #else /* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */ /* PX4IOAR: 0.00838095238 */ /* FMU standalone: 1/(10 / (47+10)) * (3.3 / 4095) = 0.00459340659 */ /* FMU with PX4IOAR: (3.3f * 52.0f / 5.0f / 4095.0f) */ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.00459340659f); -PARAM_DEFINE_FLOAT(BAT_C_SCALING, 0.001f); // TODO set correct default value #endif +PARAM_DEFINE_FLOAT(BAT_C_SCALING, 0.0124); /* scaling for 3DR power brick */ PARAM_DEFINE_INT32(RC_MAP_ROLL, 1); PARAM_DEFINE_INT32(RC_MAP_PITCH, 2); From f30ae8c9f3b2f97322f5f4b2f6dcebb6277cd9c0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 14 Jan 2014 12:29:55 +1100 Subject: [PATCH 37/54] Added MTD erase command --- src/systemcmds/mtd/mtd.c | 31 ++++++++++++++++++++++++++++++- 1 file changed, 30 insertions(+), 1 deletion(-) diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c index c9a9ea97a9..590c1fb30f 100644 --- a/src/systemcmds/mtd/mtd.c +++ b/src/systemcmds/mtd/mtd.c @@ -77,6 +77,7 @@ int mtd_main(int argc, char *argv[]) static void mtd_attach(void); static void mtd_start(char *partition_names[], unsigned n_partitions); static void mtd_test(void); +static void mtd_erase(char *partition_names[], unsigned n_partitions); static bool attached = false; static bool started = false; @@ -102,9 +103,16 @@ int mtd_main(int argc, char *argv[]) if (!strcmp(argv[1], "test")) mtd_test(); + + if (!strcmp(argv[1], "erase")) { + if (argc < 3) { + errx(1, "usage: mtd erase "); + } + mtd_erase(argv + 2, argc - 2); + } } - errx(1, "expected a command, try 'start' or 'test'"); + errx(1, "expected a command, try 'start', 'erase' or 'test'"); } struct mtd_dev_s *ramtron_initialize(FAR struct spi_dev_s *dev); @@ -247,4 +255,25 @@ mtd_test(void) exit(1); } +static void +mtd_erase(char *partition_names[], unsigned n_partitions) +{ + uint8_t v[64]; + memset(v, 0xFF, sizeof(v)); + for (uint8_t i = 0; i < n_partitions; i++) { + uint32_t count = 0; + printf("Erasing %s\n", partition_names[i]); + int fd = open(partition_names[i], O_WRONLY); + if (fd == -1) { + errx(1, "Failed to open partition"); + } + while (write(fd, &v, sizeof(v)) == sizeof(v)) { + count += sizeof(v); + } + printf("Erased %lu bytes\n", (unsigned long)count); + close(fd); + } + exit(0); +} + #endif From 85ca6e699118ce93a927a740082472285b59e3bf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 14 Jan 2014 07:34:35 +0100 Subject: [PATCH 38/54] Eliminated magic number --- src/systemcmds/mtd/mtd.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c index 590c1fb30f..baef9dcccd 100644 --- a/src/systemcmds/mtd/mtd.c +++ b/src/systemcmds/mtd/mtd.c @@ -225,7 +225,7 @@ mtd_start(char *partition_names[], unsigned n_partitions) /* Initialize to provide an FTL block driver on the MTD FLASH interface */ - snprintf(blockname, 32, "/dev/mtdblock%d", i); + snprintf(blockname, sizeof(blockname), "/dev/mtdblock%d", i); ret = ftl_initialize(i, part[i]); From d5d035b9ea03775c735a938a3573772a6ed59836 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 14 Jan 2014 07:42:01 +0100 Subject: [PATCH 39/54] Pruned old RAMTRON interface --- src/systemcmds/ramtron/module.mk | 6 - src/systemcmds/ramtron/ramtron.c | 279 ------------------------------- 2 files changed, 285 deletions(-) delete mode 100644 src/systemcmds/ramtron/module.mk delete mode 100644 src/systemcmds/ramtron/ramtron.c diff --git a/src/systemcmds/ramtron/module.mk b/src/systemcmds/ramtron/module.mk deleted file mode 100644 index e4eb1d143c..0000000000 --- a/src/systemcmds/ramtron/module.mk +++ /dev/null @@ -1,6 +0,0 @@ -# -# RAMTRON file system driver -# - -MODULE_COMMAND = ramtron -SRCS = ramtron.c diff --git a/src/systemcmds/ramtron/ramtron.c b/src/systemcmds/ramtron/ramtron.c deleted file mode 100644 index 03c713987a..0000000000 --- a/src/systemcmds/ramtron/ramtron.c +++ /dev/null @@ -1,279 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file ramtron.c - * - * ramtron service and utility app. - */ - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include - -#include "systemlib/systemlib.h" -#include "systemlib/param/param.h" -#include "systemlib/err.h" - -__EXPORT int ramtron_main(int argc, char *argv[]); - -#ifndef CONFIG_MTD_RAMTRON - -/* create a fake command with decent message to not confuse users */ -int ramtron_main(int argc, char *argv[]) -{ - errx(1, "RAMTRON not enabled, skipping."); -} -#else - -static void ramtron_attach(void); -static void ramtron_start(void); -static void ramtron_erase(void); -static void ramtron_ioctl(unsigned operation); -static void ramtron_save(const char *name); -static void ramtron_load(const char *name); -static void ramtron_test(void); - -static bool attached = false; -static bool started = false; -static struct mtd_dev_s *ramtron_mtd; - -int ramtron_main(int argc, char *argv[]) -{ - if (argc >= 2) { - if (!strcmp(argv[1], "start")) - ramtron_start(); - - if (!strcmp(argv[1], "save_param")) - ramtron_save(argv[2]); - - if (!strcmp(argv[1], "load_param")) - ramtron_load(argv[2]); - - if (!strcmp(argv[1], "erase")) - ramtron_erase(); - - if (!strcmp(argv[1], "test")) - ramtron_test(); - - if (0) { /* these actually require a file on the filesystem... */ - - if (!strcmp(argv[1], "reformat")) - ramtron_ioctl(FIOC_REFORMAT); - - if (!strcmp(argv[1], "repack")) - ramtron_ioctl(FIOC_OPTIMIZE); - } - } - - errx(1, "expected a command, try 'start'\n\t'save_param /ramtron/parameters'\n\t'load_param /ramtron/parameters'\n\t'erase'\n"); -} - -struct mtd_dev_s *ramtron_initialize(FAR struct spi_dev_s *dev); - - -static void -ramtron_attach(void) -{ - /* find the right spi */ - struct spi_dev_s *spi = up_spiinitialize(2); - /* this resets the spi bus, set correct bus speed again */ - // xxx set in ramtron driver, leave this out -// SPI_SETFREQUENCY(spi, 4000000); - SPI_SETFREQUENCY(spi, 375000000); - SPI_SETBITS(spi, 8); - SPI_SETMODE(spi, SPIDEV_MODE3); - SPI_SELECT(spi, SPIDEV_FLASH, false); - - if (spi == NULL) - errx(1, "failed to locate spi bus"); - - /* start the MTD driver, attempt 5 times */ - for (int i = 0; i < 5; i++) { - ramtron_mtd = ramtron_initialize(spi); - if (ramtron_mtd) { - /* abort on first valid result */ - if (i > 0) { - warnx("warning: ramtron needed %d attempts to attach", i+1); - } - break; - } - } - - /* if last attempt is still unsuccessful, abort */ - if (ramtron_mtd == NULL) - errx(1, "failed to initialize ramtron driver"); - - attached = true; -} - -static void -ramtron_start(void) -{ - int ret; - - if (started) - errx(1, "ramtron already mounted"); - - if (!attached) - ramtron_attach(); - - /* start NXFFS */ - ret = nxffs_initialize(ramtron_mtd); - - if (ret < 0) - errx(1, "failed to initialize NXFFS - erase ramtron to reformat"); - - /* mount the ramtron */ - ret = mount(NULL, "/ramtron", "nxffs", 0, NULL); - - if (ret < 0) - errx(1, "failed to mount /ramtron - erase ramtron to reformat"); - - started = true; - warnx("mounted ramtron at /ramtron"); - exit(0); -} - -//extern int at24c_nuke(void); - -static void -ramtron_erase(void) -{ - if (!attached) - ramtron_attach(); - -// if (at24c_nuke()) - errx(1, "erase failed"); - - errx(0, "erase done, reboot now"); -} - -static void -ramtron_ioctl(unsigned operation) -{ - int fd; - - fd = open("/ramtron/.", 0); - - if (fd < 0) - err(1, "open /ramtron"); - - if (ioctl(fd, operation, 0) < 0) - err(1, "ioctl"); - - exit(0); -} - -static void -ramtron_save(const char *name) -{ - if (!started) - errx(1, "must be started first"); - - if (!name) - err(1, "missing argument for device name, try '/ramtron/parameters'"); - - warnx("WARNING: 'ramtron save_param' deprecated - use 'param save' instead"); - - /* delete the file in case it exists */ - unlink(name); - - /* create the file */ - int fd = open(name, O_WRONLY | O_CREAT | O_EXCL); - - if (fd < 0) - err(1, "opening '%s' failed", name); - - int result = param_export(fd, false); - close(fd); - - if (result < 0) { - unlink(name); - errx(1, "error exporting to '%s'", name); - } - - exit(0); -} - -static void -ramtron_load(const char *name) -{ - if (!started) - errx(1, "must be started first"); - - if (!name) - err(1, "missing argument for device name, try '/ramtron/parameters'"); - - warnx("WARNING: 'ramtron load_param' deprecated - use 'param load' instead"); - - int fd = open(name, O_RDONLY); - - if (fd < 0) - err(1, "open '%s'", name); - - int result = param_load(fd); - close(fd); - - if (result < 0) - errx(1, "error importing from '%s'", name); - - exit(0); -} - -//extern void at24c_test(void); - -static void -ramtron_test(void) -{ -// at24c_test(); - exit(0); -} - -#endif From ba26fc32c9ad84404fc0989fa26072b2812d87ce Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 14 Jan 2014 07:49:33 +0100 Subject: [PATCH 40/54] Enabled EEPROM as MTD backend device --- src/systemcmds/mtd/24xxxx_mtd.c | 571 ++++++++++++++++++++++++++++++++ src/systemcmds/mtd/module.mk | 2 +- src/systemcmds/mtd/mtd.c | 54 ++- 3 files changed, 619 insertions(+), 8 deletions(-) create mode 100644 src/systemcmds/mtd/24xxxx_mtd.c diff --git a/src/systemcmds/mtd/24xxxx_mtd.c b/src/systemcmds/mtd/24xxxx_mtd.c new file mode 100644 index 0000000000..e34be44e31 --- /dev/null +++ b/src/systemcmds/mtd/24xxxx_mtd.c @@ -0,0 +1,571 @@ +/************************************************************************************ + * Driver for 24xxxx-style I2C EEPROMs. + * + * Adapted from: + * + * drivers/mtd/at24xx.c + * Driver for I2C-based at24cxx EEPROM(at24c32,at24c64,at24c128,at24c256) + * + * Copyright (C) 2011 Li Zhuoyi. All rights reserved. + * Author: Li Zhuoyi + * History: 0.1 2011-08-20 initial version + * + * 2011-11-1 Added support for larger MTD block sizes: Hal Glenn + * + * Derived from drivers/mtd/m25px.c + * + * Copyright (C) 2009-2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ************************************************************************************/ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "systemlib/perf_counter.h" + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ + +/* + * Configuration is as for the AT24 driver, but the CONFIG_MTD_AT24XX define should be + * omitted in order to avoid building the AT24XX driver as well. + */ + +/* As a minimum, the size of the AT24 part and its 7-bit I2C address are required. */ + +#ifndef CONFIG_AT24XX_SIZE +# warning "Assuming AT24 size 64" +# define CONFIG_AT24XX_SIZE 64 +#endif +#ifndef CONFIG_AT24XX_ADDR +# warning "Assuming AT24 address of 0x50" +# define CONFIG_AT24XX_ADDR 0x50 +#endif + +/* Get the part configuration based on the size configuration */ + +#if CONFIG_AT24XX_SIZE == 32 +# define AT24XX_NPAGES 128 +# define AT24XX_PAGESIZE 32 +#elif CONFIG_AT24XX_SIZE == 48 +# define AT24XX_NPAGES 192 +# define AT24XX_PAGESIZE 32 +#elif CONFIG_AT24XX_SIZE == 64 +# define AT24XX_NPAGES 256 +# define AT24XX_PAGESIZE 32 +#elif CONFIG_AT24XX_SIZE == 128 +# define AT24XX_NPAGES 256 +# define AT24XX_PAGESIZE 64 +#elif CONFIG_AT24XX_SIZE == 256 +# define AT24XX_NPAGES 512 +# define AT24XX_PAGESIZE 64 +#endif + +/* For applications where a file system is used on the AT24, the tiny page sizes + * will result in very inefficient FLASH usage. In such cases, it is better if + * blocks are comprised of "clusters" of pages so that the file system block + * size is, say, 256 or 512 bytes. In any event, the block size *must* be an + * even multiple of the pages. + */ + +#ifndef CONFIG_AT24XX_MTD_BLOCKSIZE +# warning "Assuming driver block size is the same as the FLASH page size" +# define CONFIG_AT24XX_MTD_BLOCKSIZE AT24XX_PAGESIZE +#endif + +/* The AT24 does not respond on the bus during write cycles, so we depend on a long + * timeout to detect problems. The max program time is typically ~5ms. + */ +#ifndef CONFIG_AT24XX_WRITE_TIMEOUT_MS +# define CONFIG_AT24XX_WRITE_TIMEOUT_MS 20 +#endif + +/************************************************************************************ + * Private Types + ************************************************************************************/ + +/* This type represents the state of the MTD device. The struct mtd_dev_s + * must appear at the beginning of the definition so that you can freely + * cast between pointers to struct mtd_dev_s and struct at24c_dev_s. + */ + +struct at24c_dev_s { + struct mtd_dev_s mtd; /* MTD interface */ + FAR struct i2c_dev_s *dev; /* Saved I2C interface instance */ + uint8_t addr; /* I2C address */ + uint16_t pagesize; /* 32, 63 */ + uint16_t npages; /* 128, 256, 512, 1024 */ + + perf_counter_t perf_reads; + perf_counter_t perf_writes; + perf_counter_t perf_resets; + perf_counter_t perf_read_retries; + perf_counter_t perf_read_errors; + perf_counter_t perf_write_errors; +}; + +/************************************************************************************ + * Private Function Prototypes + ************************************************************************************/ + +/* MTD driver methods */ + +static int at24c_erase(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks); +static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock, + size_t nblocks, FAR uint8_t *buf); +static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, + size_t nblocks, FAR const uint8_t *buf); +static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg); + +void at24c_test(void); + +/************************************************************************************ + * Private Data + ************************************************************************************/ + +/* At present, only a single AT24 part is supported. In this case, a statically + * allocated state structure may be used. + */ + +static struct at24c_dev_s g_at24c; + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +static int at24c_eraseall(FAR struct at24c_dev_s *priv) +{ + int startblock = 0; + uint8_t buf[AT24XX_PAGESIZE + 2]; + + struct i2c_msg_s msgv[1] = { + { + .addr = priv->addr, + .flags = 0, + .buffer = &buf[0], + .length = sizeof(buf), + } + }; + + memset(&buf[2], 0xff, priv->pagesize); + + for (startblock = 0; startblock < priv->npages; startblock++) { + uint16_t offset = startblock * priv->pagesize; + buf[1] = offset & 0xff; + buf[0] = (offset >> 8) & 0xff; + + while (I2C_TRANSFER(priv->dev, &msgv[0], 1) < 0) { + fvdbg("erase stall\n"); + usleep(10000); + } + } + + return OK; +} + +/************************************************************************************ + * Name: at24c_erase + ************************************************************************************/ + +static int at24c_erase(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks) +{ + /* EEprom need not erase */ + + return (int)nblocks; +} + +/************************************************************************************ + * Name: at24c_test + ************************************************************************************/ + +void at24c_test(void) +{ + uint8_t buf[CONFIG_AT24XX_MTD_BLOCKSIZE]; + unsigned count = 0; + unsigned errors = 0; + + for (count = 0; count < 10000; count++) { + ssize_t result = at24c_bread(&g_at24c.mtd, 0, 1, buf); + if (result == ERROR) { + if (errors++ > 2) { + vdbg("too many errors\n"); + return; + } + } else if (result != 1) { + vdbg("unexpected %u\n", result); + } + if ((count % 100) == 0) + vdbg("test %u errors %u\n", count, errors); + } +} + +/************************************************************************************ + * Name: at24c_bread + ************************************************************************************/ + +static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock, + size_t nblocks, FAR uint8_t *buffer) +{ + FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev; + size_t blocksleft; + uint8_t addr[2]; + int ret; + + struct i2c_msg_s msgv[2] = { + { + .addr = priv->addr, + .flags = 0, + .buffer = &addr[0], + .length = sizeof(addr), + }, + { + .addr = priv->addr, + .flags = I2C_M_READ, + .buffer = 0, + .length = priv->pagesize, + } + }; + +#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE + startblock *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); + nblocks *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); +#endif + blocksleft = nblocks; + + fvdbg("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks); + + if (startblock >= priv->npages) { + return 0; + } + + if (startblock + nblocks > priv->npages) { + nblocks = priv->npages - startblock; + } + + while (blocksleft-- > 0) { + uint16_t offset = startblock * priv->pagesize; + unsigned tries = CONFIG_AT24XX_WRITE_TIMEOUT_MS; + + addr[1] = offset & 0xff; + addr[0] = (offset >> 8) & 0xff; + msgv[1].buffer = buffer; + + for (;;) { + + perf_begin(priv->perf_reads); + ret = I2C_TRANSFER(priv->dev, &msgv[0], 2); + perf_end(priv->perf_reads); + + if (ret >= 0) + break; + + fvdbg("read stall"); + usleep(1000); + + /* We should normally only be here on the first read after + * a write. + * + * XXX maybe do special first-read handling with optional + * bus reset as well? + */ + perf_count(priv->perf_read_retries); + + if (--tries == 0) { + perf_count(priv->perf_read_errors); + return ERROR; + } + } + + startblock++; + buffer += priv->pagesize; + } + +#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE + return nblocks / (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); +#else + return nblocks; +#endif +} + +/************************************************************************************ + * Name: at24c_bwrite + * + * Operates on MTD block's and translates to FLASH pages + * + ************************************************************************************/ + +static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks, + FAR const uint8_t *buffer) +{ + FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev; + size_t blocksleft; + uint8_t buf[AT24XX_PAGESIZE + 2]; + int ret; + + struct i2c_msg_s msgv[1] = { + { + .addr = priv->addr, + .flags = 0, + .buffer = &buf[0], + .length = sizeof(buf), + } + }; + +#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE + startblock *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); + nblocks *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); +#endif + blocksleft = nblocks; + + if (startblock >= priv->npages) { + return 0; + } + + if (startblock + nblocks > priv->npages) { + nblocks = priv->npages - startblock; + } + + fvdbg("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks); + + while (blocksleft-- > 0) { + uint16_t offset = startblock * priv->pagesize; + unsigned tries = CONFIG_AT24XX_WRITE_TIMEOUT_MS; + + buf[1] = offset & 0xff; + buf[0] = (offset >> 8) & 0xff; + memcpy(&buf[2], buffer, priv->pagesize); + + for (;;) { + + perf_begin(priv->perf_writes); + ret = I2C_TRANSFER(priv->dev, &msgv[0], 1); + perf_end(priv->perf_writes); + + if (ret >= 0) + break; + + fvdbg("write stall"); + usleep(1000); + + /* We expect to see a number of retries per write cycle as we + * poll for write completion. + */ + if (--tries == 0) { + perf_count(priv->perf_write_errors); + return ERROR; + } + } + + startblock++; + buffer += priv->pagesize; + } + +#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE + return nblocks / (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); +#else + return nblocks; +#endif +} + +/************************************************************************************ + * Name: at24c_ioctl + ************************************************************************************/ + +static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg) +{ + FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev; + int ret = -EINVAL; /* Assume good command with bad parameters */ + + fvdbg("cmd: %d \n", cmd); + + switch (cmd) { + case MTDIOC_GEOMETRY: { + FAR struct mtd_geometry_s *geo = (FAR struct mtd_geometry_s *)((uintptr_t)arg); + + if (geo) { + /* Populate the geometry structure with information need to know + * the capacity and how to access the device. + * + * NOTE: that the device is treated as though it where just an array + * of fixed size blocks. That is most likely not true, but the client + * will expect the device logic to do whatever is necessary to make it + * appear so. + * + * blocksize: + * May be user defined. The block size for the at24XX devices may be + * larger than the page size in order to better support file systems. + * The read and write functions translate BLOCKS to pages for the + * small flash devices + * erasesize: + * It has to be at least as big as the blocksize, bigger serves no + * purpose. + * neraseblocks + * Note that the device size is in kilobits and must be scaled by + * 1024 / 8 + */ + +#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE + geo->blocksize = CONFIG_AT24XX_MTD_BLOCKSIZE; + geo->erasesize = CONFIG_AT24XX_MTD_BLOCKSIZE; + geo->neraseblocks = (CONFIG_AT24XX_SIZE * 1024 / 8) / CONFIG_AT24XX_MTD_BLOCKSIZE; +#else + geo->blocksize = priv->pagesize; + geo->erasesize = priv->pagesize; + geo->neraseblocks = priv->npages; +#endif + ret = OK; + + fvdbg("blocksize: %d erasesize: %d neraseblocks: %d\n", + geo->blocksize, geo->erasesize, geo->neraseblocks); + } + } + break; + + case MTDIOC_BULKERASE: + ret = at24c_eraseall(priv); + break; + + case MTDIOC_XIPBASE: + default: + ret = -ENOTTY; /* Bad command */ + break; + } + + return ret; +} + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: at24c_initialize + * + * Description: + * Create an initialize MTD device instance. MTD devices are not registered + * in the file system, but are created as instances that can be bound to + * other functions (such as a block or character driver front end). + * + ************************************************************************************/ + +FAR struct mtd_dev_s *at24c_initialize(FAR struct i2c_dev_s *dev) { + FAR struct at24c_dev_s *priv; + + fvdbg("dev: %p\n", dev); + + /* Allocate a state structure (we allocate the structure instead of using + * a fixed, static allocation so that we can handle multiple FLASH devices. + * The current implementation would handle only one FLASH part per I2C + * device (only because of the SPIDEV_FLASH definition) and so would have + * to be extended to handle multiple FLASH parts on the same I2C bus. + */ + + priv = &g_at24c; + + if (priv) { + /* Initialize the allocated structure */ + + priv->addr = CONFIG_AT24XX_ADDR; + priv->pagesize = AT24XX_PAGESIZE; + priv->npages = AT24XX_NPAGES; + + priv->mtd.erase = at24c_erase; + priv->mtd.bread = at24c_bread; + priv->mtd.bwrite = at24c_bwrite; + priv->mtd.ioctl = at24c_ioctl; + priv->dev = dev; + + priv->perf_reads = perf_alloc(PC_ELAPSED, "EEPROM read"); + priv->perf_writes = perf_alloc(PC_ELAPSED, "EEPROM write"); + priv->perf_resets = perf_alloc(PC_COUNT, "EEPROM reset"); + priv->perf_read_retries = perf_alloc(PC_COUNT, "EEPROM read retries"); + priv->perf_read_errors = perf_alloc(PC_COUNT, "EEPROM read errors"); + priv->perf_write_errors = perf_alloc(PC_COUNT, "EEPROM write errors"); + } + + /* attempt to read to validate device is present */ + unsigned char buf[5]; + uint8_t addrbuf[2] = {0, 0}; + + struct i2c_msg_s msgv[2] = { + { + .addr = priv->addr, + .flags = 0, + .buffer = &addrbuf[0], + .length = sizeof(addrbuf), + }, + { + .addr = priv->addr, + .flags = I2C_M_READ, + .buffer = &buf[0], + .length = sizeof(buf), + } + }; + + perf_begin(priv->perf_reads); + int ret = I2C_TRANSFER(priv->dev, &msgv[0], 2); + perf_end(priv->perf_reads); + + if (ret < 0) { + return NULL; + } + + /* Return the implementation-specific state structure as the MTD device */ + + fvdbg("Return %p\n", priv); + return (FAR struct mtd_dev_s *)priv; +} + +/* + * XXX: debug hackery + */ +int at24c_nuke(void) +{ + return at24c_eraseall(&g_at24c); +} diff --git a/src/systemcmds/mtd/module.mk b/src/systemcmds/mtd/module.mk index 6866565977..b3fceceb5c 100644 --- a/src/systemcmds/mtd/module.mk +++ b/src/systemcmds/mtd/module.mk @@ -3,4 +3,4 @@ # MODULE_COMMAND = mtd -SRCS = mtd.c +SRCS = mtd.c 24xxxx_mtd.c diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c index baef9dcccd..e89b6e5aa3 100644 --- a/src/systemcmds/mtd/mtd.c +++ b/src/systemcmds/mtd/mtd.c @@ -64,17 +64,20 @@ __EXPORT int mtd_main(int argc, char *argv[]); -#ifndef CONFIG_MTD_RAMTRON +#ifndef CONFIG_MTD /* create a fake command with decent warnx to not confuse users */ int mtd_main(int argc, char *argv[]) { - errx(1, "RAMTRON not enabled, skipping."); + errx(1, "MTD not enabled, skipping."); } #else -static void mtd_attach(void); +#ifdef CONFIG_MTD_RAMTRON +static void ramtron_attach(void); +#endif +static void at24xxx_attach(void); static void mtd_start(char *partition_names[], unsigned n_partitions); static void mtd_test(void); static void mtd_erase(char *partition_names[], unsigned n_partitions); @@ -119,8 +122,9 @@ struct mtd_dev_s *ramtron_initialize(FAR struct spi_dev_s *dev); struct mtd_dev_s *mtd_partition(FAR struct mtd_dev_s *mtd, off_t firstblock, off_t nblocks); +#ifdef CONFIG_MTD_RAMTRON static void -mtd_attach(void) +ramtron_attach(void) { /* find the right spi */ struct spi_dev_s *spi = up_spiinitialize(2); @@ -133,7 +137,7 @@ mtd_attach(void) if (spi == NULL) errx(1, "failed to locate spi bus"); - /* start the MTD driver, attempt 5 times */ + /* start the RAMTRON driver, attempt 5 times */ for (int i = 0; i < 5; i++) { mtd_dev = ramtron_initialize(spi); @@ -153,6 +157,37 @@ mtd_attach(void) attached = true; } +#endif + +static void +at24xxx_attach(void) +{ + /* find the right I2C */ + struct i2c_dev_s *i2c = up_i2cinitialize(PX4_I2C_BUS_ONBOARD); + /* this resets the I2C bus, set correct bus speed again */ + I2C_SETFREQUENCY(i2c, 400000); + + if (i2c == NULL) + errx(1, "failed to locate I2C bus"); + + /* start the MTD driver, attempt 5 times */ + for (int i = 0; i < 5; i++) { + mtd_dev = at24c_initialize(i2c); + if (mtd_dev) { + /* abort on first valid result */ + if (i > 0) { + warnx("warning: EEPROM needed %d attempts to attach", i+1); + } + break; + } + } + + /* if last attempt is still unsuccessful, abort */ + if (mtd_dev == NULL) + errx(1, "failed to initialize EEPROM driver"); + + attached = true; +} static void mtd_start(char *partition_names[], unsigned n_partitions) @@ -162,8 +197,13 @@ mtd_start(char *partition_names[], unsigned n_partitions) if (started) errx(1, "mtd already mounted"); - if (!attached) - mtd_attach(); + if (!attached) { + #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + at24xxx_attach(); + #else + ramtron_attach(); + #endif + } if (!mtd_dev) { warnx("ERROR: Failed to create RAMTRON FRAM MTD instance"); From 1e0f292566b2a80557a2727e99b74990c75c90d9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 14 Jan 2014 08:57:32 +0100 Subject: [PATCH 41/54] Disabling EEPROM and old RAMTRON driver --- makefiles/config_px4fmu-v1_backside.mk | 3 +-- makefiles/config_px4fmu-v1_default.mk | 3 +-- makefiles/config_px4fmu-v1_test.mk | 1 + makefiles/config_px4fmu-v2_default.mk | 1 - 4 files changed, 3 insertions(+), 5 deletions(-) diff --git a/makefiles/config_px4fmu-v1_backside.mk b/makefiles/config_px4fmu-v1_backside.mk index c86beaccae..ba26ef28f3 100644 --- a/makefiles/config_px4fmu-v1_backside.mk +++ b/makefiles/config_px4fmu-v1_backside.mk @@ -38,8 +38,7 @@ MODULES += modules/sensors # # System commands # -MODULES += systemcmds/eeprom -MODULES += systemcmds/ramtron +MODULES += systemcmds/mtd MODULES += systemcmds/bl_update MODULES += systemcmds/boardinfo MODULES += systemcmds/i2c diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index d0733ec53d..a269d01ab3 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -42,8 +42,7 @@ MODULES += modules/sensors # # System commands # -MODULES += systemcmds/eeprom -MODULES += systemcmds/ramtron +MODULES += systemcmds/mtd MODULES += systemcmds/bl_update MODULES += systemcmds/boardinfo MODULES += systemcmds/i2c diff --git a/makefiles/config_px4fmu-v1_test.mk b/makefiles/config_px4fmu-v1_test.mk index 41e8b95ff5..4ba93fa749 100644 --- a/makefiles/config_px4fmu-v1_test.mk +++ b/makefiles/config_px4fmu-v1_test.mk @@ -22,6 +22,7 @@ MODULES += systemcmds/perf MODULES += systemcmds/reboot MODULES += systemcmds/tests MODULES += systemcmds/nshterm +MODULES += systemcmds/mtd # # Library modules diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 99f26c3f39..e90312226c 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -46,7 +46,6 @@ MODULES += modules/sensors # # System commands # -MODULES += systemcmds/ramtron MODULES += systemcmds/bl_update MODULES += systemcmds/boardinfo MODULES += systemcmds/mixer From 993f7212104814db917943bedb471990f040a532 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 14 Jan 2014 08:57:52 +0100 Subject: [PATCH 42/54] Deleting old eeprom driver directory --- src/systemcmds/eeprom/24xxxx_mtd.c | 571 ----------------------------- src/systemcmds/eeprom/eeprom.c | 265 ------------- src/systemcmds/eeprom/module.mk | 39 -- 3 files changed, 875 deletions(-) delete mode 100644 src/systemcmds/eeprom/24xxxx_mtd.c delete mode 100644 src/systemcmds/eeprom/eeprom.c delete mode 100644 src/systemcmds/eeprom/module.mk diff --git a/src/systemcmds/eeprom/24xxxx_mtd.c b/src/systemcmds/eeprom/24xxxx_mtd.c deleted file mode 100644 index e34be44e31..0000000000 --- a/src/systemcmds/eeprom/24xxxx_mtd.c +++ /dev/null @@ -1,571 +0,0 @@ -/************************************************************************************ - * Driver for 24xxxx-style I2C EEPROMs. - * - * Adapted from: - * - * drivers/mtd/at24xx.c - * Driver for I2C-based at24cxx EEPROM(at24c32,at24c64,at24c128,at24c256) - * - * Copyright (C) 2011 Li Zhuoyi. All rights reserved. - * Author: Li Zhuoyi - * History: 0.1 2011-08-20 initial version - * - * 2011-11-1 Added support for larger MTD block sizes: Hal Glenn - * - * Derived from drivers/mtd/m25px.c - * - * Copyright (C) 2009-2011 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name NuttX nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ************************************************************************************/ - -/************************************************************************************ - * Included Files - ************************************************************************************/ - -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include "systemlib/perf_counter.h" - -/************************************************************************************ - * Pre-processor Definitions - ************************************************************************************/ - -/* - * Configuration is as for the AT24 driver, but the CONFIG_MTD_AT24XX define should be - * omitted in order to avoid building the AT24XX driver as well. - */ - -/* As a minimum, the size of the AT24 part and its 7-bit I2C address are required. */ - -#ifndef CONFIG_AT24XX_SIZE -# warning "Assuming AT24 size 64" -# define CONFIG_AT24XX_SIZE 64 -#endif -#ifndef CONFIG_AT24XX_ADDR -# warning "Assuming AT24 address of 0x50" -# define CONFIG_AT24XX_ADDR 0x50 -#endif - -/* Get the part configuration based on the size configuration */ - -#if CONFIG_AT24XX_SIZE == 32 -# define AT24XX_NPAGES 128 -# define AT24XX_PAGESIZE 32 -#elif CONFIG_AT24XX_SIZE == 48 -# define AT24XX_NPAGES 192 -# define AT24XX_PAGESIZE 32 -#elif CONFIG_AT24XX_SIZE == 64 -# define AT24XX_NPAGES 256 -# define AT24XX_PAGESIZE 32 -#elif CONFIG_AT24XX_SIZE == 128 -# define AT24XX_NPAGES 256 -# define AT24XX_PAGESIZE 64 -#elif CONFIG_AT24XX_SIZE == 256 -# define AT24XX_NPAGES 512 -# define AT24XX_PAGESIZE 64 -#endif - -/* For applications where a file system is used on the AT24, the tiny page sizes - * will result in very inefficient FLASH usage. In such cases, it is better if - * blocks are comprised of "clusters" of pages so that the file system block - * size is, say, 256 or 512 bytes. In any event, the block size *must* be an - * even multiple of the pages. - */ - -#ifndef CONFIG_AT24XX_MTD_BLOCKSIZE -# warning "Assuming driver block size is the same as the FLASH page size" -# define CONFIG_AT24XX_MTD_BLOCKSIZE AT24XX_PAGESIZE -#endif - -/* The AT24 does not respond on the bus during write cycles, so we depend on a long - * timeout to detect problems. The max program time is typically ~5ms. - */ -#ifndef CONFIG_AT24XX_WRITE_TIMEOUT_MS -# define CONFIG_AT24XX_WRITE_TIMEOUT_MS 20 -#endif - -/************************************************************************************ - * Private Types - ************************************************************************************/ - -/* This type represents the state of the MTD device. The struct mtd_dev_s - * must appear at the beginning of the definition so that you can freely - * cast between pointers to struct mtd_dev_s and struct at24c_dev_s. - */ - -struct at24c_dev_s { - struct mtd_dev_s mtd; /* MTD interface */ - FAR struct i2c_dev_s *dev; /* Saved I2C interface instance */ - uint8_t addr; /* I2C address */ - uint16_t pagesize; /* 32, 63 */ - uint16_t npages; /* 128, 256, 512, 1024 */ - - perf_counter_t perf_reads; - perf_counter_t perf_writes; - perf_counter_t perf_resets; - perf_counter_t perf_read_retries; - perf_counter_t perf_read_errors; - perf_counter_t perf_write_errors; -}; - -/************************************************************************************ - * Private Function Prototypes - ************************************************************************************/ - -/* MTD driver methods */ - -static int at24c_erase(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks); -static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock, - size_t nblocks, FAR uint8_t *buf); -static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, - size_t nblocks, FAR const uint8_t *buf); -static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg); - -void at24c_test(void); - -/************************************************************************************ - * Private Data - ************************************************************************************/ - -/* At present, only a single AT24 part is supported. In this case, a statically - * allocated state structure may be used. - */ - -static struct at24c_dev_s g_at24c; - -/************************************************************************************ - * Private Functions - ************************************************************************************/ - -static int at24c_eraseall(FAR struct at24c_dev_s *priv) -{ - int startblock = 0; - uint8_t buf[AT24XX_PAGESIZE + 2]; - - struct i2c_msg_s msgv[1] = { - { - .addr = priv->addr, - .flags = 0, - .buffer = &buf[0], - .length = sizeof(buf), - } - }; - - memset(&buf[2], 0xff, priv->pagesize); - - for (startblock = 0; startblock < priv->npages; startblock++) { - uint16_t offset = startblock * priv->pagesize; - buf[1] = offset & 0xff; - buf[0] = (offset >> 8) & 0xff; - - while (I2C_TRANSFER(priv->dev, &msgv[0], 1) < 0) { - fvdbg("erase stall\n"); - usleep(10000); - } - } - - return OK; -} - -/************************************************************************************ - * Name: at24c_erase - ************************************************************************************/ - -static int at24c_erase(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks) -{ - /* EEprom need not erase */ - - return (int)nblocks; -} - -/************************************************************************************ - * Name: at24c_test - ************************************************************************************/ - -void at24c_test(void) -{ - uint8_t buf[CONFIG_AT24XX_MTD_BLOCKSIZE]; - unsigned count = 0; - unsigned errors = 0; - - for (count = 0; count < 10000; count++) { - ssize_t result = at24c_bread(&g_at24c.mtd, 0, 1, buf); - if (result == ERROR) { - if (errors++ > 2) { - vdbg("too many errors\n"); - return; - } - } else if (result != 1) { - vdbg("unexpected %u\n", result); - } - if ((count % 100) == 0) - vdbg("test %u errors %u\n", count, errors); - } -} - -/************************************************************************************ - * Name: at24c_bread - ************************************************************************************/ - -static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock, - size_t nblocks, FAR uint8_t *buffer) -{ - FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev; - size_t blocksleft; - uint8_t addr[2]; - int ret; - - struct i2c_msg_s msgv[2] = { - { - .addr = priv->addr, - .flags = 0, - .buffer = &addr[0], - .length = sizeof(addr), - }, - { - .addr = priv->addr, - .flags = I2C_M_READ, - .buffer = 0, - .length = priv->pagesize, - } - }; - -#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE - startblock *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); - nblocks *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); -#endif - blocksleft = nblocks; - - fvdbg("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks); - - if (startblock >= priv->npages) { - return 0; - } - - if (startblock + nblocks > priv->npages) { - nblocks = priv->npages - startblock; - } - - while (blocksleft-- > 0) { - uint16_t offset = startblock * priv->pagesize; - unsigned tries = CONFIG_AT24XX_WRITE_TIMEOUT_MS; - - addr[1] = offset & 0xff; - addr[0] = (offset >> 8) & 0xff; - msgv[1].buffer = buffer; - - for (;;) { - - perf_begin(priv->perf_reads); - ret = I2C_TRANSFER(priv->dev, &msgv[0], 2); - perf_end(priv->perf_reads); - - if (ret >= 0) - break; - - fvdbg("read stall"); - usleep(1000); - - /* We should normally only be here on the first read after - * a write. - * - * XXX maybe do special first-read handling with optional - * bus reset as well? - */ - perf_count(priv->perf_read_retries); - - if (--tries == 0) { - perf_count(priv->perf_read_errors); - return ERROR; - } - } - - startblock++; - buffer += priv->pagesize; - } - -#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE - return nblocks / (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); -#else - return nblocks; -#endif -} - -/************************************************************************************ - * Name: at24c_bwrite - * - * Operates on MTD block's and translates to FLASH pages - * - ************************************************************************************/ - -static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks, - FAR const uint8_t *buffer) -{ - FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev; - size_t blocksleft; - uint8_t buf[AT24XX_PAGESIZE + 2]; - int ret; - - struct i2c_msg_s msgv[1] = { - { - .addr = priv->addr, - .flags = 0, - .buffer = &buf[0], - .length = sizeof(buf), - } - }; - -#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE - startblock *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); - nblocks *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); -#endif - blocksleft = nblocks; - - if (startblock >= priv->npages) { - return 0; - } - - if (startblock + nblocks > priv->npages) { - nblocks = priv->npages - startblock; - } - - fvdbg("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks); - - while (blocksleft-- > 0) { - uint16_t offset = startblock * priv->pagesize; - unsigned tries = CONFIG_AT24XX_WRITE_TIMEOUT_MS; - - buf[1] = offset & 0xff; - buf[0] = (offset >> 8) & 0xff; - memcpy(&buf[2], buffer, priv->pagesize); - - for (;;) { - - perf_begin(priv->perf_writes); - ret = I2C_TRANSFER(priv->dev, &msgv[0], 1); - perf_end(priv->perf_writes); - - if (ret >= 0) - break; - - fvdbg("write stall"); - usleep(1000); - - /* We expect to see a number of retries per write cycle as we - * poll for write completion. - */ - if (--tries == 0) { - perf_count(priv->perf_write_errors); - return ERROR; - } - } - - startblock++; - buffer += priv->pagesize; - } - -#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE - return nblocks / (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); -#else - return nblocks; -#endif -} - -/************************************************************************************ - * Name: at24c_ioctl - ************************************************************************************/ - -static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg) -{ - FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev; - int ret = -EINVAL; /* Assume good command with bad parameters */ - - fvdbg("cmd: %d \n", cmd); - - switch (cmd) { - case MTDIOC_GEOMETRY: { - FAR struct mtd_geometry_s *geo = (FAR struct mtd_geometry_s *)((uintptr_t)arg); - - if (geo) { - /* Populate the geometry structure with information need to know - * the capacity and how to access the device. - * - * NOTE: that the device is treated as though it where just an array - * of fixed size blocks. That is most likely not true, but the client - * will expect the device logic to do whatever is necessary to make it - * appear so. - * - * blocksize: - * May be user defined. The block size for the at24XX devices may be - * larger than the page size in order to better support file systems. - * The read and write functions translate BLOCKS to pages for the - * small flash devices - * erasesize: - * It has to be at least as big as the blocksize, bigger serves no - * purpose. - * neraseblocks - * Note that the device size is in kilobits and must be scaled by - * 1024 / 8 - */ - -#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE - geo->blocksize = CONFIG_AT24XX_MTD_BLOCKSIZE; - geo->erasesize = CONFIG_AT24XX_MTD_BLOCKSIZE; - geo->neraseblocks = (CONFIG_AT24XX_SIZE * 1024 / 8) / CONFIG_AT24XX_MTD_BLOCKSIZE; -#else - geo->blocksize = priv->pagesize; - geo->erasesize = priv->pagesize; - geo->neraseblocks = priv->npages; -#endif - ret = OK; - - fvdbg("blocksize: %d erasesize: %d neraseblocks: %d\n", - geo->blocksize, geo->erasesize, geo->neraseblocks); - } - } - break; - - case MTDIOC_BULKERASE: - ret = at24c_eraseall(priv); - break; - - case MTDIOC_XIPBASE: - default: - ret = -ENOTTY; /* Bad command */ - break; - } - - return ret; -} - -/************************************************************************************ - * Public Functions - ************************************************************************************/ - -/************************************************************************************ - * Name: at24c_initialize - * - * Description: - * Create an initialize MTD device instance. MTD devices are not registered - * in the file system, but are created as instances that can be bound to - * other functions (such as a block or character driver front end). - * - ************************************************************************************/ - -FAR struct mtd_dev_s *at24c_initialize(FAR struct i2c_dev_s *dev) { - FAR struct at24c_dev_s *priv; - - fvdbg("dev: %p\n", dev); - - /* Allocate a state structure (we allocate the structure instead of using - * a fixed, static allocation so that we can handle multiple FLASH devices. - * The current implementation would handle only one FLASH part per I2C - * device (only because of the SPIDEV_FLASH definition) and so would have - * to be extended to handle multiple FLASH parts on the same I2C bus. - */ - - priv = &g_at24c; - - if (priv) { - /* Initialize the allocated structure */ - - priv->addr = CONFIG_AT24XX_ADDR; - priv->pagesize = AT24XX_PAGESIZE; - priv->npages = AT24XX_NPAGES; - - priv->mtd.erase = at24c_erase; - priv->mtd.bread = at24c_bread; - priv->mtd.bwrite = at24c_bwrite; - priv->mtd.ioctl = at24c_ioctl; - priv->dev = dev; - - priv->perf_reads = perf_alloc(PC_ELAPSED, "EEPROM read"); - priv->perf_writes = perf_alloc(PC_ELAPSED, "EEPROM write"); - priv->perf_resets = perf_alloc(PC_COUNT, "EEPROM reset"); - priv->perf_read_retries = perf_alloc(PC_COUNT, "EEPROM read retries"); - priv->perf_read_errors = perf_alloc(PC_COUNT, "EEPROM read errors"); - priv->perf_write_errors = perf_alloc(PC_COUNT, "EEPROM write errors"); - } - - /* attempt to read to validate device is present */ - unsigned char buf[5]; - uint8_t addrbuf[2] = {0, 0}; - - struct i2c_msg_s msgv[2] = { - { - .addr = priv->addr, - .flags = 0, - .buffer = &addrbuf[0], - .length = sizeof(addrbuf), - }, - { - .addr = priv->addr, - .flags = I2C_M_READ, - .buffer = &buf[0], - .length = sizeof(buf), - } - }; - - perf_begin(priv->perf_reads); - int ret = I2C_TRANSFER(priv->dev, &msgv[0], 2); - perf_end(priv->perf_reads); - - if (ret < 0) { - return NULL; - } - - /* Return the implementation-specific state structure as the MTD device */ - - fvdbg("Return %p\n", priv); - return (FAR struct mtd_dev_s *)priv; -} - -/* - * XXX: debug hackery - */ -int at24c_nuke(void) -{ - return at24c_eraseall(&g_at24c); -} diff --git a/src/systemcmds/eeprom/eeprom.c b/src/systemcmds/eeprom/eeprom.c deleted file mode 100644 index 2aed80e011..0000000000 --- a/src/systemcmds/eeprom/eeprom.c +++ /dev/null @@ -1,265 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file eeprom.c - * - * EEPROM service and utility app. - */ - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include - -#include "systemlib/systemlib.h" -#include "systemlib/param/param.h" -#include "systemlib/err.h" - -#ifndef PX4_I2C_BUS_ONBOARD -# error PX4_I2C_BUS_ONBOARD not defined, cannot locate onboard EEPROM -#endif - -__EXPORT int eeprom_main(int argc, char *argv[]); - -static void eeprom_attach(void); -static void eeprom_start(void); -static void eeprom_erase(void); -static void eeprom_ioctl(unsigned operation); -static void eeprom_save(const char *name); -static void eeprom_load(const char *name); -static void eeprom_test(void); - -static bool attached = false; -static bool started = false; -static struct mtd_dev_s *eeprom_mtd; - -int eeprom_main(int argc, char *argv[]) -{ - if (argc >= 2) { - if (!strcmp(argv[1], "start")) - eeprom_start(); - - if (!strcmp(argv[1], "save_param")) - eeprom_save(argv[2]); - - if (!strcmp(argv[1], "load_param")) - eeprom_load(argv[2]); - - if (!strcmp(argv[1], "erase")) - eeprom_erase(); - - if (!strcmp(argv[1], "test")) - eeprom_test(); - - if (0) { /* these actually require a file on the filesystem... */ - - if (!strcmp(argv[1], "reformat")) - eeprom_ioctl(FIOC_REFORMAT); - - if (!strcmp(argv[1], "repack")) - eeprom_ioctl(FIOC_OPTIMIZE); - } - } - - errx(1, "expected a command, try 'start'\n\t'save_param /eeprom/parameters'\n\t'load_param /eeprom/parameters'\n\t'erase'\n"); -} - - -static void -eeprom_attach(void) -{ - /* find the right I2C */ - struct i2c_dev_s *i2c = up_i2cinitialize(PX4_I2C_BUS_ONBOARD); - /* this resets the I2C bus, set correct bus speed again */ - I2C_SETFREQUENCY(i2c, 400000); - - if (i2c == NULL) - errx(1, "failed to locate I2C bus"); - - /* start the MTD driver, attempt 5 times */ - for (int i = 0; i < 5; i++) { - eeprom_mtd = at24c_initialize(i2c); - if (eeprom_mtd) { - /* abort on first valid result */ - if (i > 0) { - warnx("warning: EEPROM needed %d attempts to attach", i+1); - } - break; - } - } - - /* if last attempt is still unsuccessful, abort */ - if (eeprom_mtd == NULL) - errx(1, "failed to initialize EEPROM driver"); - - attached = true; -} - -static void -eeprom_start(void) -{ - int ret; - - if (started) - errx(1, "EEPROM already mounted"); - - if (!attached) - eeprom_attach(); - - /* start NXFFS */ - ret = nxffs_initialize(eeprom_mtd); - - if (ret < 0) - errx(1, "failed to initialize NXFFS - erase EEPROM to reformat"); - - /* mount the EEPROM */ - ret = mount(NULL, "/eeprom", "nxffs", 0, NULL); - - if (ret < 0) - errx(1, "failed to mount /eeprom - erase EEPROM to reformat"); - - started = true; - warnx("mounted EEPROM at /eeprom"); - exit(0); -} - -extern int at24c_nuke(void); - -static void -eeprom_erase(void) -{ - if (!attached) - eeprom_attach(); - - if (at24c_nuke()) - errx(1, "erase failed"); - - errx(0, "erase done, reboot now"); -} - -static void -eeprom_ioctl(unsigned operation) -{ - int fd; - - fd = open("/eeprom/.", 0); - - if (fd < 0) - err(1, "open /eeprom"); - - if (ioctl(fd, operation, 0) < 0) - err(1, "ioctl"); - - exit(0); -} - -static void -eeprom_save(const char *name) -{ - if (!started) - errx(1, "must be started first"); - - if (!name) - err(1, "missing argument for device name, try '/eeprom/parameters'"); - - warnx("WARNING: 'eeprom save_param' deprecated - use 'param save' instead"); - - /* delete the file in case it exists */ - unlink(name); - - /* create the file */ - int fd = open(name, O_WRONLY | O_CREAT | O_EXCL); - - if (fd < 0) - err(1, "opening '%s' failed", name); - - int result = param_export(fd, false); - close(fd); - - if (result < 0) { - unlink(name); - errx(1, "error exporting to '%s'", name); - } - - exit(0); -} - -static void -eeprom_load(const char *name) -{ - if (!started) - errx(1, "must be started first"); - - if (!name) - err(1, "missing argument for device name, try '/eeprom/parameters'"); - - warnx("WARNING: 'eeprom load_param' deprecated - use 'param load' instead"); - - int fd = open(name, O_RDONLY); - - if (fd < 0) - err(1, "open '%s'", name); - - int result = param_load(fd); - close(fd); - - if (result < 0) - errx(1, "error importing from '%s'", name); - - exit(0); -} - -extern void at24c_test(void); - -static void -eeprom_test(void) -{ - at24c_test(); - exit(0); -} diff --git a/src/systemcmds/eeprom/module.mk b/src/systemcmds/eeprom/module.mk deleted file mode 100644 index 07f3945be2..0000000000 --- a/src/systemcmds/eeprom/module.mk +++ /dev/null @@ -1,39 +0,0 @@ -############################################################################ -# -# Copyright (c) 2012, 2013 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 -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# EEPROM file system driver -# - -MODULE_COMMAND = eeprom -SRCS = 24xxxx_mtd.c eeprom.c From 33b84186e3492660007c5b46c5e988e8acef60fa Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 14 Jan 2014 08:58:30 +0100 Subject: [PATCH 43/54] Patching up MPU6K pin disable defines --- src/drivers/boards/px4fmu-v2/board_config.h | 4 ++-- src/drivers/px4fmu/fmu.cpp | 11 +++++++++++ 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index 4972e69147..264d911f32 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -87,7 +87,7 @@ __BEGIN_DECLS #define GPIO_GYRO_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN0) #define GPIO_MAG_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN1) #define GPIO_ACCEL_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN4) -#define GPIO_EXTI_MPU_DRDY (GPIO_INPUT|GPIO_PULLDOWN|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15) +#define GPIO_EXTI_MPU_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15) /* SPI1 off */ #define GPIO_SPI1_SCK_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN5) @@ -98,7 +98,7 @@ __BEGIN_DECLS #define GPIO_SPI_CS_GYRO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN13) #define GPIO_SPI_CS_ACCEL_MAG_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN15) #define GPIO_SPI_CS_BARO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTD|GPIO_PIN7) -#define GPIO_SPI_CS_MPU (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN2) +#define GPIO_SPI_CS_MPU_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN2) /* SPI chip selects */ #define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index b28d318d7e..4b1b0ed0bb 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1108,10 +1108,12 @@ PX4FMU::sensor_reset(int ms) stm32_configgpio(GPIO_SPI_CS_GYRO_OFF); stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG_OFF); stm32_configgpio(GPIO_SPI_CS_BARO_OFF); + stm32_configgpio(GPIO_SPI_CS_MPU_OFF); stm32_gpiowrite(GPIO_SPI_CS_GYRO_OFF, 0); stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG_OFF, 0); stm32_gpiowrite(GPIO_SPI_CS_BARO_OFF, 0); + stm32_gpiowrite(GPIO_SPI_CS_MPU_OFF, 0); stm32_configgpio(GPIO_SPI1_SCK_OFF); stm32_configgpio(GPIO_SPI1_MISO_OFF); @@ -1124,10 +1126,12 @@ PX4FMU::sensor_reset(int ms) stm32_configgpio(GPIO_GYRO_DRDY_OFF); stm32_configgpio(GPIO_MAG_DRDY_OFF); stm32_configgpio(GPIO_ACCEL_DRDY_OFF); + stm32_configgpio(GPIO_EXTI_MPU_DRDY_OFF); stm32_gpiowrite(GPIO_GYRO_DRDY_OFF, 0); stm32_gpiowrite(GPIO_MAG_DRDY_OFF, 0); stm32_gpiowrite(GPIO_ACCEL_DRDY_OFF, 0); + stm32_gpiowrite(GPIO_EXTI_MPU_DRDY_OFF, 0); /* set the sensor rail off */ stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN); @@ -1160,6 +1164,13 @@ PX4FMU::sensor_reset(int ms) stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); + + // // XXX bring up the EXTI pins again + // stm32_configgpio(GPIO_GYRO_DRDY); + // stm32_configgpio(GPIO_MAG_DRDY); + // stm32_configgpio(GPIO_ACCEL_DRDY); + // stm32_configgpio(GPIO_EXTI_MPU_DRDY); + #endif #endif } From 202e89de911a8acddcec400b0c2956f5590d8bc8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 14 Jan 2014 08:58:58 +0100 Subject: [PATCH 44/54] Introducing mtd status command, fixing compile errors for I2C setup --- src/systemcmds/mtd/mtd.c | 117 +++++++++++++++++++++++++++++---------- 1 file changed, 88 insertions(+), 29 deletions(-) diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c index e89b6e5aa3..41da63750c 100644 --- a/src/systemcmds/mtd/mtd.c +++ b/src/systemcmds/mtd/mtd.c @@ -62,6 +62,8 @@ #include "systemlib/param/param.h" #include "systemlib/err.h" +#include + __EXPORT int mtd_main(int argc, char *argv[]); #ifndef CONFIG_MTD @@ -76,15 +78,25 @@ int mtd_main(int argc, char *argv[]) #ifdef CONFIG_MTD_RAMTRON static void ramtron_attach(void); +#else + +#ifndef PX4_I2C_BUS_ONBOARD +# error PX4_I2C_BUS_ONBOARD not defined, cannot locate onboard EEPROM #endif + static void at24xxx_attach(void); +#endif static void mtd_start(char *partition_names[], unsigned n_partitions); static void mtd_test(void); static void mtd_erase(char *partition_names[], unsigned n_partitions); +static void mtd_print_info(); +static int mtd_get_geometry(unsigned long *blocksize, unsigned long *erasesize, unsigned long *neraseblocks, + unsigned *blkpererase, unsigned *nblocks, unsigned *partsize, unsigned n_partitions); static bool attached = false; static bool started = false; static struct mtd_dev_s *mtd_dev; +static unsigned n_partitions_current = 0; /* note, these will be equally sized */ static char *partition_names_default[] = {"/fs/mtd_params", "/fs/mtd_waypoints"}; @@ -107,6 +119,9 @@ int mtd_main(int argc, char *argv[]) if (!strcmp(argv[1], "test")) mtd_test(); + if (!strcmp(argv[1], "status")) + mtd_status(); + if (!strcmp(argv[1], "erase")) { if (argc < 3) { errx(1, "usage: mtd erase "); @@ -157,7 +172,7 @@ ramtron_attach(void) attached = true; } -#endif +#else static void at24xxx_attach(void) @@ -188,6 +203,7 @@ at24xxx_attach(void) attached = true; } +#endif static void mtd_start(char *partition_names[], unsigned n_partitions) @@ -210,34 +226,12 @@ mtd_start(char *partition_names[], unsigned n_partitions) exit(1); } + unsigned long blocksize, erasesize, neraseblocks; + unsigned blkpererase, nblocks, partsize; - /* Get the geometry of the FLASH device */ - - FAR struct mtd_geometry_s geo; - - ret = mtd_dev->ioctl(mtd_dev, MTDIOC_GEOMETRY, (unsigned long)((uintptr_t)&geo)); - - if (ret < 0) { - warnx("ERROR: mtd->ioctl failed: %d", ret); + ret = mtd_get_geometry(&blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize, n_partitions); + if (ret) exit(3); - } - - warnx("Flash Geometry:"); - warnx(" blocksize: %lu", (unsigned long)geo.blocksize); - warnx(" erasesize: %lu", (unsigned long)geo.erasesize); - warnx(" neraseblocks: %lu", (unsigned long)geo.neraseblocks); - - /* Determine the size of each partition. Make each partition an even - * multiple of the erase block size (perhaps not using some space at the - * end of the FLASH). - */ - - unsigned blkpererase = geo.erasesize / geo.blocksize; - unsigned nblocks = (geo.neraseblocks / n_partitions) * blkpererase; - unsigned partsize = nblocks * geo.blocksize; - - warnx(" No. partitions: %u", n_partitions); - warnx(" Partition size: %lu Blocks (%lu bytes)", (unsigned long)nblocks, (unsigned long)partsize); /* Now create MTD FLASH partitions */ @@ -284,18 +278,83 @@ mtd_start(char *partition_names[], unsigned n_partitions) } } + n_partitions_current = n_partitions; + started = true; exit(0); } -static void +int mtd_get_geometry(unsigned long *blocksize, unsigned long *erasesize, unsigned long *neraseblocks, + unsigned *blkpererase, unsigned *nblocks, unsigned *partsize, unsigned n_partitions) +{ + /* Get the geometry of the FLASH device */ + + FAR struct mtd_geometry_s geo; + + int ret = mtd_dev->ioctl(mtd_dev, MTDIOC_GEOMETRY, (unsigned long)((uintptr_t)&geo)); + + if (ret < 0) { + warnx("ERROR: mtd->ioctl failed: %d", ret); + return ret; + } + + *blocksize = geo.blocksize; + *erasesize = geo.blocksize; + *neraseblocks = geo.neraseblocks; + + /* Determine the size of each partition. Make each partition an even + * multiple of the erase block size (perhaps not using some space at the + * end of the FLASH). + */ + + *blkpererase = geo.erasesize / geo.blocksize; + *nblocks = (geo.neraseblocks / n_partitions) * *blkpererase; + *partsize = *nblocks * geo.blocksize; + + return ret; +} + +void mtd_print_info() +{ + if (!attached) + exit(1); + + unsigned long blocksize, erasesize, neraseblocks; + unsigned blkpererase, nblocks, partsize; + + int ret = mtd_get_geometry(&blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize, n_partitions_current); + if (ret) + exit(3); + + warnx("Flash Geometry:"); + + printf(" blocksize: %lu\n", blocksize); + printf(" erasesize: %lu\n", erasesize); + printf(" neraseblocks: %lu\n", neraseblocks); + printf(" No. partitions: %u\n", n_partitions_current); + printf(" Partition size: %u Blocks (%u bytes)\n", nblocks, partsize); + printf(" TOTAL SIZE: %u KiB\n", neraseblocks * erasesize / 1024); + +} + +void mtd_test(void) { warnx("This test routine does not test anything yet!"); exit(1); } -static void +void +mtd_status(void) +{ + if (!attached) + errx(1, "MTD driver not started"); + + mtd_print_info(); + exit(0); +} + +void mtd_erase(char *partition_names[], unsigned n_partitions) { uint8_t v[64]; From 778cbcb5cc28240aa1caeae4dd02c2a39567a0e1 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 15 Jan 2014 19:27:03 +1100 Subject: [PATCH 45/54] mtd: fixed creation and erase of a single partition --- src/systemcmds/mtd/mtd.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c index 41da63750c..5525a8f337 100644 --- a/src/systemcmds/mtd/mtd.c +++ b/src/systemcmds/mtd/mtd.c @@ -108,9 +108,8 @@ int mtd_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { /* start mapping according to user request */ - if (argc > 3) { + if (argc >= 3) { mtd_start(argv + 2, argc - 2); - } else { mtd_start(partition_names_default, n_partitions_default); } @@ -123,10 +122,11 @@ int mtd_main(int argc, char *argv[]) mtd_status(); if (!strcmp(argv[1], "erase")) { - if (argc < 3) { - errx(1, "usage: mtd erase "); + if (argc >= 3) { + mtd_erase(argv + 2, argc - 2); + } else { + mtd_erase(partition_names_default, n_partitions_default); } - mtd_erase(argv + 2, argc - 2); } } From 84ad289e0ae8d2bd32cba5c22a27a35132b5c863 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 15 Jan 2014 17:20:08 +0100 Subject: [PATCH 46/54] Improved test suite, now features a MTD device test --- src/systemcmds/tests/module.mk | 3 +- src/systemcmds/tests/test_file.c | 2 +- src/systemcmds/tests/test_mtd.c | 296 ++++++++++++++++++++++++++++++ src/systemcmds/tests/tests.h | 1 + src/systemcmds/tests/tests_main.c | 1 + 5 files changed, 301 insertions(+), 2 deletions(-) create mode 100644 src/systemcmds/tests/test_mtd.c diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk index 6a05ac3c61..beb9ad13d3 100644 --- a/src/systemcmds/tests/module.mk +++ b/src/systemcmds/tests/module.mk @@ -30,4 +30,5 @@ SRCS = test_adc.c \ test_ppm_loopback.c \ test_rc.c \ test_conv.cpp \ - test_mount.c + test_mount.c \ + test_mtd.c diff --git a/src/systemcmds/tests/test_file.c b/src/systemcmds/tests/test_file.c index 83d09dd5e8..96be1e8df5 100644 --- a/src/systemcmds/tests/test_file.c +++ b/src/systemcmds/tests/test_file.c @@ -54,7 +54,7 @@ #include "tests.h" -int check_user_abort(int fd); +static int check_user_abort(int fd); int check_user_abort(int fd) { /* check if user wants to abort */ diff --git a/src/systemcmds/tests/test_mtd.c b/src/systemcmds/tests/test_mtd.c new file mode 100644 index 0000000000..bcd24e5c90 --- /dev/null +++ b/src/systemcmds/tests/test_mtd.c @@ -0,0 +1,296 @@ +/**************************************************************************** + * + * 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file test_mtd.c + * + * Param storage / file test. + * + * @author Lorenz Meier + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "tests.h" + +#define PARAM_FILE_NAME "/fs/mtd_params" + +static int check_user_abort(int fd); + +int check_user_abort(int fd) { + /* check if user wants to abort */ + char c; + + struct pollfd fds; + int ret; + fds.fd = 0; /* stdin */ + fds.events = POLLIN; + ret = poll(&fds, 1, 0); + + if (ret > 0) { + + read(0, &c, 1); + + switch (c) { + case 0x03: // ctrl-c + case 0x1b: // esc + case 'c': + case 'q': + { + warnx("Test aborted."); + fsync(fd); + close(fd); + return OK; + /* not reached */ + } + } + } + + return 1; +} + +int +test_mtd(int argc, char *argv[]) +{ + unsigned iterations= 0; + + /* check if microSD card is mounted */ + struct stat buffer; + if (stat(PARAM_FILE_NAME, &buffer)) { + warnx("file %s not found, aborting MTD test", PARAM_FILE_NAME); + return 1; + } + + // XXX get real storage space here + unsigned file_size = 4096; + + /* perform tests for a range of chunk sizes */ + unsigned chunk_sizes[] = {256, 512, 4096}; + + for (unsigned c = 0; c < (sizeof(chunk_sizes) / sizeof(chunk_sizes[0])); c++) { + + printf("\n====== FILE TEST: %u bytes chunks ======\n", chunk_sizes[c]); + + uint8_t write_buf[chunk_sizes[c]] __attribute__((aligned(64))); + + /* fill write buffer with known values */ + for (int i = 0; i < sizeof(write_buf); i++) { + /* this will wrap, but we just need a known value with spacing */ + write_buf[i] = i+11; + } + + uint8_t read_buf[chunk_sizes[c]] __attribute__((aligned(64))); + hrt_abstime start, end; + + int fd = open(PARAM_FILE_NAME, O_WRONLY); + + warnx("testing unaligned writes - please wait.."); + + iterations = file_size / chunk_sizes[c]; + + start = hrt_absolute_time(); + for (unsigned i = 0; i < iterations; i++) { + int wret = write(fd, write_buf, chunk_sizes[c]); + + if (wret != chunk_sizes[c]) { + warn("WRITE ERROR!"); + + return 1; + } + + fsync(fd); + + if (!check_user_abort(fd)) + return OK; + + } + end = hrt_absolute_time(); + + close(fd); + fd = open(PARAM_FILE_NAME, O_RDONLY); + + /* read back data for validation */ + for (unsigned i = 0; i < iterations; i++) { + int rret = read(fd, read_buf, chunk_sizes[c]); + + if (rret != chunk_sizes[c]) { + warnx("READ ERROR!"); + return 1; + } + + /* compare value */ + bool compare_ok = true; + + for (int j = 0; j < chunk_sizes[c]; j++) { + if (read_buf[j] != write_buf[j]) { + warnx("COMPARISON ERROR: byte %d", j); + compare_ok = false; + break; + } + } + + if (!compare_ok) { + warnx("ABORTING FURTHER COMPARISON DUE TO ERROR"); + return 1; + } + + if (!check_user_abort(fd)) + return OK; + + } + + close(fd); + + printf("RESULT: OK! No readback errors.\n\n"); + + // /* + // * ALIGNED WRITES AND UNALIGNED READS + // */ + + + // fd = open(PARAM_FILE_NAME, O_WRONLY); + + // warnx("testing aligned writes - please wait.. (CTRL^C to abort)"); + + // start = hrt_absolute_time(); + // for (unsigned i = 0; i < iterations; i++) { + // int wret = write(fd, write_buf, chunk_sizes[c]); + + // if (wret != chunk_sizes[c]) { + // warnx("WRITE ERROR!"); + // return 1; + // } + + // if (!check_user_abort(fd)) + // return OK; + + // } + + // fsync(fd); + + // warnx("reading data aligned.."); + + // close(fd); + // fd = open(PARAM_FILE_NAME, O_RDONLY); + + // bool align_read_ok = true; + + // /* read back data unaligned */ + // for (unsigned i = 0; i < iterations; i++) { + // int rret = read(fd, read_buf, chunk_sizes[c]); + + // if (rret != chunk_sizes[c]) { + // warnx("READ ERROR!"); + // return 1; + // } + + // /* compare value */ + // bool compare_ok = true; + + // for (int j = 0; j < chunk_sizes[c]; j++) { + // if (read_buf[j] != write_buf[j]) { + // warnx("COMPARISON ERROR: byte %d: %u != %u", j, (unsigned int)read_buf[j], (unsigned int)write_buf[j]); + // align_read_ok = false; + // break; + // } + + // if (!check_user_abort(fd)) + // return OK; + // } + + // if (!align_read_ok) { + // warnx("ABORTING FURTHER COMPARISON DUE TO ERROR"); + // return 1; + // } + + // } + + // warnx("align read result: %s\n", (align_read_ok) ? "OK" : "ERROR"); + + // warnx("reading data unaligned.."); + + // close(fd); + // fd = open(PARAM_FILE_NAME, O_RDONLY); + + // bool unalign_read_ok = true; + // int unalign_read_err_count = 0; + + // memset(read_buf, 0, sizeof(read_buf)); + + // /* read back data unaligned */ + // for (unsigned i = 0; i < iterations; i++) { + // int rret = read(fd, read_buf + a, chunk_sizes[c]); + + // if (rret != chunk_sizes[c]) { + // warnx("READ ERROR!"); + // return 1; + // } + + // for (int j = 0; j < chunk_sizes[c]; j++) { + + // if ((read_buf + a)[j] != write_buf[j]) { + // warnx("COMPARISON ERROR: byte %d, align shift: %d: %u != %u", j, a, (unsigned int)read_buf[j + a], (unsigned int)write_buf[j]); + // unalign_read_ok = false; + // unalign_read_err_count++; + + // if (unalign_read_err_count > 10) + // break; + // } + + // if (!check_user_abort(fd)) + // return OK; + // } + + // if (!unalign_read_ok) { + // warnx("ABORTING FURTHER COMPARISON DUE TO ERROR"); + // return 1; + // } + + // } + + // close(fd); + } + + return 0; +} diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h index ff05cc322d..223edc14a9 100644 --- a/src/systemcmds/tests/tests.h +++ b/src/systemcmds/tests/tests.h @@ -111,6 +111,7 @@ extern int test_mixer(int argc, char *argv[]); extern int test_rc(int argc, char *argv[]); extern int test_conv(int argc, char *argv[]); extern int test_mount(int argc, char *argv[]); +extern int test_mtd(int argc, char *argv[]); __END_DECLS diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index 0d3dabc00e..77a4df618b 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -108,6 +108,7 @@ const struct { {"rc", test_rc, OPT_NOJIGTEST | OPT_NOALLTEST}, {"conv", test_conv, OPT_NOJIGTEST | OPT_NOALLTEST}, {"mount", test_mount, OPT_NOJIGTEST | OPT_NOALLTEST}, + {"mtd", test_mtd, 0}, {"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST}, {NULL, NULL, 0} }; From 352dea675435794d90f75d4a3c013d2afc439933 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 15 Jan 2014 20:04:11 +0100 Subject: [PATCH 47/54] Remove outdated configs, clean up pwm limit compilation --- makefiles/config_px4fmu-v1_backside.mk | 145 ------------------------- makefiles/config_px4fmu-v1_test.mk | 49 --------- src/drivers/px4fmu/module.mk | 3 +- src/modules/systemlib/module.mk | 3 +- 4 files changed, 3 insertions(+), 197 deletions(-) delete mode 100644 makefiles/config_px4fmu-v1_backside.mk delete mode 100644 makefiles/config_px4fmu-v1_test.mk diff --git a/makefiles/config_px4fmu-v1_backside.mk b/makefiles/config_px4fmu-v1_backside.mk deleted file mode 100644 index ba26ef28f3..0000000000 --- a/makefiles/config_px4fmu-v1_backside.mk +++ /dev/null @@ -1,145 +0,0 @@ -# -# Makefile for the px4fmu_default configuration -# - -# -# Use the configuration's ROMFS. -# -ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common -ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v1_default.bin - -# -# Board support modules -# -MODULES += drivers/device -MODULES += drivers/stm32 -MODULES += drivers/stm32/adc -MODULES += drivers/stm32/tone_alarm -MODULES += drivers/led -MODULES += drivers/px4io -MODULES += drivers/px4fmu -MODULES += drivers/boards/px4fmu-v1 -MODULES += drivers/ardrone_interface -MODULES += drivers/l3gd20 -MODULES += drivers/bma180 -MODULES += drivers/mpu6000 -MODULES += drivers/hmc5883 -MODULES += drivers/ms5611 -MODULES += drivers/gps -MODULES += drivers/hil -MODULES += drivers/blinkm -MODULES += drivers/rgbled -MODULES += drivers/roboclaw -MODULES += drivers/airspeed -MODULES += drivers/ets_airspeed -MODULES += drivers/meas_airspeed -MODULES += modules/sensors - -# -# System commands -# -MODULES += systemcmds/mtd -MODULES += systemcmds/bl_update -MODULES += systemcmds/boardinfo -MODULES += systemcmds/i2c -MODULES += systemcmds/mixer -MODULES += systemcmds/param -MODULES += systemcmds/perf -MODULES += systemcmds/preflight_check -MODULES += systemcmds/pwm -MODULES += systemcmds/esc_calib -MODULES += systemcmds/reboot -MODULES += systemcmds/top -MODULES += systemcmds/tests -MODULES += systemcmds/config -MODULES += systemcmds/nshterm - -# -# General system control -# -MODULES += modules/commander -MODULES += modules/navigator -MODULES += modules/mavlink -MODULES += modules/mavlink_onboard -MODULES += modules/gpio_led - -# -# Estimation modules (EKF/ SO3 / other filters) -# -MODULES += modules/att_pos_estimator_ekf - -# -# Vehicle Control -# -MODULES += modules/fixedwing_backside - -# -# Logging -# -MODULES += modules/sdlog2 - -# -# Unit tests -# -#MODULES += modules/unit_test -#MODULES += modules/commander/commander_tests - -# -# Library modules -# -MODULES += modules/systemlib -MODULES += modules/systemlib/mixer -MODULES += modules/controllib -MODULES += modules/uORB - -# -# Libraries -# -LIBRARIES += lib/mathlib/CMSIS -MODULES += lib/mathlib -MODULES += lib/mathlib/math/filter -MODULES += lib/ecl -MODULES += lib/external_lgpl -MODULES += lib/geo -MODULES += lib/conversion - -# -# Demo apps -# -#MODULES += examples/math_demo -# Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/hello_sky -#MODULES += examples/px4_simple_app - -# Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/daemon -#MODULES += examples/px4_daemon_app - -# Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/debug_values -#MODULES += examples/px4_mavlink_debug - -# Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control -#MODULES += examples/fixedwing_control - -# Hardware test -#MODULES += examples/hwtest - -# -# Transitional support - add commands from the NuttX export archive. -# -# In general, these should move to modules over time. -# -# Each entry here is ... but we use a helper macro -# to make the table a bit more readable. -# -define _B - $(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4) -endef - -# command priority stack entrypoint -BUILTIN_COMMANDS := \ - $(call _B, sercon, , 2048, sercon_main ) \ - $(call _B, serdis, , 2048, serdis_main ) \ - $(call _B, sysinfo, , 2048, sysinfo_main ) diff --git a/makefiles/config_px4fmu-v1_test.mk b/makefiles/config_px4fmu-v1_test.mk deleted file mode 100644 index 4ba93fa749..0000000000 --- a/makefiles/config_px4fmu-v1_test.mk +++ /dev/null @@ -1,49 +0,0 @@ -# -# Makefile for the px4fmu_default configuration -# - -# -# Use the configuration's ROMFS. -# -ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_test -ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v1_default.bin - -# -# Board support modules -# -MODULES += drivers/device -MODULES += drivers/stm32 -MODULES += drivers/stm32/adc -MODULES += drivers/stm32/tone_alarm -MODULES += drivers/led -MODULES += drivers/boards/px4fmu-v1 -MODULES += drivers/px4io -MODULES += systemcmds/perf -MODULES += systemcmds/reboot -MODULES += systemcmds/tests -MODULES += systemcmds/nshterm -MODULES += systemcmds/mtd - -# -# Library modules -# -MODULES += modules/systemlib -MODULES += modules/systemlib/mixer -MODULES += modules/uORB - -# -# Transitional support - add commands from the NuttX export archive. -# -# In general, these should move to modules over time. -# -# Each entry here is ... but we use a helper macro -# to make the table a bit more readable. -# -define _B - $(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4) -endef - -# command priority stack entrypoint -BUILTIN_COMMANDS := \ - $(call _B, sercon, , 2048, sercon_main ) \ - $(call _B, serdis, , 2048, serdis_main ) diff --git a/src/drivers/px4fmu/module.mk b/src/drivers/px4fmu/module.mk index d918abd572..05bc7a5b33 100644 --- a/src/drivers/px4fmu/module.mk +++ b/src/drivers/px4fmu/module.mk @@ -3,5 +3,4 @@ # MODULE_COMMAND = fmu -SRCS = fmu.cpp \ - ../../modules/systemlib/pwm_limit/pwm_limit.c +SRCS = fmu.cpp diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index 8c6c300d6b..3953b757d2 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -51,5 +51,6 @@ SRCS = err.c \ mavlink_log.c \ rc_check.c \ otp.c \ - board_serial.c + board_serial.c \ + pwm_limit/pwm_limit.c From ff59aa9a0f856826682eb5099b1ec2525c5f7ba6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 16 Jan 2014 17:00:25 +1100 Subject: [PATCH 48/54] mtd: use new MTDIOC_SETSPEED ioctl set SPI speed to 10MHz --- src/systemcmds/mtd/mtd.c | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c index 5525a8f337..8bc18b3c24 100644 --- a/src/systemcmds/mtd/mtd.c +++ b/src/systemcmds/mtd/mtd.c @@ -144,7 +144,7 @@ ramtron_attach(void) /* find the right spi */ struct spi_dev_s *spi = up_spiinitialize(2); /* this resets the spi bus, set correct bus speed again */ - SPI_SETFREQUENCY(spi, 40 * 1000 * 1000); + SPI_SETFREQUENCY(spi, 10 * 1000 * 1000); SPI_SETBITS(spi, 8); SPI_SETMODE(spi, SPIDEV_MODE3); SPI_SELECT(spi, SPIDEV_FLASH, false); @@ -170,6 +170,10 @@ ramtron_attach(void) if (mtd_dev == NULL) errx(1, "failed to initialize mtd driver"); + int ret = mtd_dev->ioctl(mtd_dev, MTDIOC_SETSPEED, (unsigned long)10*1000*1000); + if (ret != OK) + warnx(1, "failed to set bus speed"); + attached = true; } #else From e5ad3c31e01680ccd18fe64d113ecf05de080e71 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 16 Jan 2014 12:46:20 +1100 Subject: [PATCH 49/54] mtd: added "mtd readtest" and "mtd rwtest" this allows for verification of MTD operation on startup --- src/systemcmds/mtd/mtd.c | 115 ++++++++++++++++++++++++++++++++++++++- 1 file changed, 113 insertions(+), 2 deletions(-) diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c index 8bc18b3c24..a2a0c109c6 100644 --- a/src/systemcmds/mtd/mtd.c +++ b/src/systemcmds/mtd/mtd.c @@ -89,6 +89,8 @@ static void at24xxx_attach(void); static void mtd_start(char *partition_names[], unsigned n_partitions); static void mtd_test(void); static void mtd_erase(char *partition_names[], unsigned n_partitions); +static void mtd_readtest(char *partition_names[], unsigned n_partitions); +static void mtd_rwtest(char *partition_names[], unsigned n_partitions); static void mtd_print_info(); static int mtd_get_geometry(unsigned long *blocksize, unsigned long *erasesize, unsigned long *neraseblocks, unsigned *blkpererase, unsigned *nblocks, unsigned *partsize, unsigned n_partitions); @@ -118,6 +120,22 @@ int mtd_main(int argc, char *argv[]) if (!strcmp(argv[1], "test")) mtd_test(); + if (!strcmp(argv[1], "readtest")) { + if (argc >= 3) { + mtd_readtest(argv + 2, argc - 2); + } else { + mtd_readtest(partition_names_default, n_partitions_default); + } + } + + if (!strcmp(argv[1], "rwtest")) { + if (argc >= 3) { + mtd_rwtest(argv + 2, argc - 2); + } else { + mtd_rwtest(partition_names_default, n_partitions_default); + } + } + if (!strcmp(argv[1], "status")) mtd_status(); @@ -130,7 +148,7 @@ int mtd_main(int argc, char *argv[]) } } - errx(1, "expected a command, try 'start', 'erase' or 'test'"); + errx(1, "expected a command, try 'start', 'erase', 'status', 'readtest', 'rwtest' or 'test'"); } struct mtd_dev_s *ramtron_initialize(FAR struct spi_dev_s *dev); @@ -318,6 +336,21 @@ int mtd_get_geometry(unsigned long *blocksize, unsigned long *erasesize, unsigne return ret; } +/* + get partition size in bytes + */ +static ssize_t mtd_get_partition_size(void) +{ + unsigned long blocksize, erasesize, neraseblocks; + unsigned blkpererase, nblocks, partsize = 0; + + int ret = mtd_get_geometry(&blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize, n_partitions_current); + if (ret != OK) { + errx(1, "Failed to get geometry"); + } + return partsize; +} + void mtd_print_info() { if (!attached) @@ -370,7 +403,7 @@ mtd_erase(char *partition_names[], unsigned n_partitions) if (fd == -1) { errx(1, "Failed to open partition"); } - while (write(fd, &v, sizeof(v)) == sizeof(v)) { + while (write(fd, v, sizeof(v)) == sizeof(v)) { count += sizeof(v); } printf("Erased %lu bytes\n", (unsigned long)count); @@ -379,4 +412,82 @@ mtd_erase(char *partition_names[], unsigned n_partitions) exit(0); } +/* + readtest is useful during startup to validate the device is + responding on the bus. It relies on the driver returning an error on + bad reads (the ramtron driver does return an error) + */ +void +mtd_readtest(char *partition_names[], unsigned n_partitions) +{ + ssize_t expected_size = mtd_get_partition_size(); + + uint8_t v[128]; + for (uint8_t i = 0; i < n_partitions; i++) { + uint32_t count = 0; + printf("reading %s expecting %u bytes\n", partition_names[i], expected_size); + int fd = open(partition_names[i], O_RDONLY); + if (fd == -1) { + errx(1, "Failed to open partition"); + } + while (read(fd, v, sizeof(v)) == sizeof(v)) { + count += sizeof(v); + } + if (count != expected_size) { + errx(1,"Failed to read partition - got %u/%u bytes", count, expected_size); + } + close(fd); + } + printf("readtest OK\n"); + exit(0); +} + +/* + rwtest is useful during startup to validate the device is + responding on the bus for both reads and writes. It reads data in + blocks and writes the data back, then reads it again, failing if the + data isn't the same + */ +void +mtd_rwtest(char *partition_names[], unsigned n_partitions) +{ + ssize_t expected_size = mtd_get_partition_size(); + + uint8_t v[128], v2[128]; + for (uint8_t i = 0; i < n_partitions; i++) { + uint32_t count = 0; + off_t offset = 0; + printf("rwtest %s testing %u bytes\n", partition_names[i], expected_size); + int fd = open(partition_names[i], O_RDWR); + if (fd == -1) { + errx(1, "Failed to open partition"); + } + while (read(fd, v, sizeof(v)) == sizeof(v)) { + count += sizeof(v); + if (lseek(fd, offset, SEEK_SET) != offset) { + errx(1, "seek failed"); + } + if (write(fd, v, sizeof(v)) != sizeof(v)) { + errx(1, "write failed"); + } + if (lseek(fd, offset, SEEK_SET) != offset) { + errx(1, "seek failed"); + } + if (read(fd, v2, sizeof(v2)) != sizeof(v2)) { + errx(1, "read failed"); + } + if (memcmp(v, v2, sizeof(v2)) != 0) { + errx(1, "memcmp failed"); + } + offset += sizeof(v); + } + if (count != expected_size) { + errx(1,"Failed to read partition - got %u/%u bytes", count, expected_size); + } + close(fd); + } + printf("rwtest OK\n"); + exit(0); +} + #endif From c304ea25077e1fd4675ef1d053cfc81e7e877b4b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 16 Jan 2014 08:37:50 +0100 Subject: [PATCH 50/54] Teached MTD test how to write back 0xFF after destructive test --- src/systemcmds/tests/test_mtd.c | 124 +++++--------------------------- 1 file changed, 17 insertions(+), 107 deletions(-) diff --git a/src/systemcmds/tests/test_mtd.c b/src/systemcmds/tests/test_mtd.c index bcd24e5c90..8a626b65cc 100644 --- a/src/systemcmds/tests/test_mtd.c +++ b/src/systemcmds/tests/test_mtd.c @@ -180,117 +180,27 @@ test_mtd(int argc, char *argv[]) } + close(fd); printf("RESULT: OK! No readback errors.\n\n"); - - // /* - // * ALIGNED WRITES AND UNALIGNED READS - // */ - - - // fd = open(PARAM_FILE_NAME, O_WRONLY); - - // warnx("testing aligned writes - please wait.. (CTRL^C to abort)"); - - // start = hrt_absolute_time(); - // for (unsigned i = 0; i < iterations; i++) { - // int wret = write(fd, write_buf, chunk_sizes[c]); - - // if (wret != chunk_sizes[c]) { - // warnx("WRITE ERROR!"); - // return 1; - // } - - // if (!check_user_abort(fd)) - // return OK; - - // } - - // fsync(fd); - - // warnx("reading data aligned.."); - - // close(fd); - // fd = open(PARAM_FILE_NAME, O_RDONLY); - - // bool align_read_ok = true; - - // /* read back data unaligned */ - // for (unsigned i = 0; i < iterations; i++) { - // int rret = read(fd, read_buf, chunk_sizes[c]); - - // if (rret != chunk_sizes[c]) { - // warnx("READ ERROR!"); - // return 1; - // } - - // /* compare value */ - // bool compare_ok = true; - - // for (int j = 0; j < chunk_sizes[c]; j++) { - // if (read_buf[j] != write_buf[j]) { - // warnx("COMPARISON ERROR: byte %d: %u != %u", j, (unsigned int)read_buf[j], (unsigned int)write_buf[j]); - // align_read_ok = false; - // break; - // } - - // if (!check_user_abort(fd)) - // return OK; - // } - - // if (!align_read_ok) { - // warnx("ABORTING FURTHER COMPARISON DUE TO ERROR"); - // return 1; - // } - - // } - - // warnx("align read result: %s\n", (align_read_ok) ? "OK" : "ERROR"); - - // warnx("reading data unaligned.."); - - // close(fd); - // fd = open(PARAM_FILE_NAME, O_RDONLY); - - // bool unalign_read_ok = true; - // int unalign_read_err_count = 0; - - // memset(read_buf, 0, sizeof(read_buf)); - - // /* read back data unaligned */ - // for (unsigned i = 0; i < iterations; i++) { - // int rret = read(fd, read_buf + a, chunk_sizes[c]); - - // if (rret != chunk_sizes[c]) { - // warnx("READ ERROR!"); - // return 1; - // } - - // for (int j = 0; j < chunk_sizes[c]; j++) { - - // if ((read_buf + a)[j] != write_buf[j]) { - // warnx("COMPARISON ERROR: byte %d, align shift: %d: %u != %u", j, a, (unsigned int)read_buf[j + a], (unsigned int)write_buf[j]); - // unalign_read_ok = false; - // unalign_read_err_count++; - - // if (unalign_read_err_count > 10) - // break; - // } - - // if (!check_user_abort(fd)) - // return OK; - // } - - // if (!unalign_read_ok) { - // warnx("ABORTING FURTHER COMPARISON DUE TO ERROR"); - // return 1; - // } - - // } - - // close(fd); } + /* fill the file with 0xFF to make it look new again */ + char ffbuf[64]; + memset(ffbuf, 0xFF, sizeof(ffbuf)); + int fd = open(PARAM_FILE_NAME, O_WRONLY); + for (int i = 0; i < file_size / sizeof(ffbuf); i++) { + int ret = write(fd, ffbuf, sizeof(ffbuf)); + + if (ret) { + warnx("ERROR! Could not fill file with 0xFF"); + close(fd); + return 1; + } + } + + (void)close(fd); + return 0; } From a0bb6674da5c0ca9c23f1db91bc9506c75242398 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 16 Jan 2014 19:03:14 +0100 Subject: [PATCH 51/54] Fix FMUs B/E led pin config --- src/drivers/boards/px4fmu-v2/board_config.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index 264d911f32..7cfca7656e 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -75,7 +75,7 @@ __BEGIN_DECLS /* PX4FMU GPIOs ***********************************************************************************/ /* LEDs */ -#define GPIO_LED1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN12) +#define GPIO_LED1 (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN12) /* External interrupts */ #define GPIO_EXTI_GYRO_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN0) From 2600c96e92de4ce8123b20210176456ec7e5332d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 16 Jan 2014 20:13:17 +0100 Subject: [PATCH 52/54] Configuring PX4IOv1 led pins --- src/drivers/boards/px4io-v1/board_config.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drivers/boards/px4io-v1/board_config.h b/src/drivers/boards/px4io-v1/board_config.h index c3f39addfe..1be4877bad 100644 --- a/src/drivers/boards/px4io-v1/board_config.h +++ b/src/drivers/boards/px4io-v1/board_config.h @@ -58,11 +58,11 @@ /* PX4IO GPIOs **********************************************************************/ /* LEDs */ -#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ +#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\ GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14) -#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ +#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\ GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN15) -#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ +#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\ GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) /* Safety switch button *************************************************************/ From 71b11d54e0cc441dabed878db270881a6308a7c7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 16 Jan 2014 20:13:35 +0100 Subject: [PATCH 53/54] Configuring PX4IOv2 led pins --- src/drivers/boards/px4io-v2/board_config.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h index 8da5552110..8152ea4d4b 100644 --- a/src/drivers/boards/px4io-v2/board_config.h +++ b/src/drivers/boards/px4io-v2/board_config.h @@ -74,9 +74,9 @@ /* LEDS **********************************************************************/ -#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14) -#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15) -#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13) +#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14) +#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15) +#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13) /* Safety switch button *******************************************************/ From e691bab71a69216273fdc253695ef849671af9a7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 16 Jan 2014 22:46:55 +0100 Subject: [PATCH 54/54] Cleaned up test output to be more readable --- src/systemcmds/tests/test_mtd.c | 35 +++++++++++++++++++++++++++------ 1 file changed, 29 insertions(+), 6 deletions(-) diff --git a/src/systemcmds/tests/test_mtd.c b/src/systemcmds/tests/test_mtd.c index 8a626b65cc..bac9efbdb3 100644 --- a/src/systemcmds/tests/test_mtd.c +++ b/src/systemcmds/tests/test_mtd.c @@ -90,6 +90,16 @@ int check_user_abort(int fd) { return 1; } +void print_fail() +{ + printf("<[T]: MTD: FAIL>\n"); +} + +void print_success() +{ + printf("<[T]: MTD: OK>\n"); +} + int test_mtd(int argc, char *argv[]) { @@ -99,6 +109,7 @@ test_mtd(int argc, char *argv[]) struct stat buffer; if (stat(PARAM_FILE_NAME, &buffer)) { warnx("file %s not found, aborting MTD test", PARAM_FILE_NAME); + print_fail(); return 1; } @@ -123,9 +134,17 @@ test_mtd(int argc, char *argv[]) uint8_t read_buf[chunk_sizes[c]] __attribute__((aligned(64))); hrt_abstime start, end; - int fd = open(PARAM_FILE_NAME, O_WRONLY); + int fd = open(PARAM_FILE_NAME, O_RDONLY); + int rret = read(fd, read_buf, chunk_sizes[c]); + close(fd); - warnx("testing unaligned writes - please wait.."); + fd = open(PARAM_FILE_NAME, O_WRONLY); + + printf("printing 2 percent of the first chunk:\n"); + for (int i = 0; i < sizeof(read_buf) / 50; i++) { + printf("%02X", read_buf[i]); + } + printf("\n"); iterations = file_size / chunk_sizes[c]; @@ -133,9 +152,9 @@ test_mtd(int argc, char *argv[]) for (unsigned i = 0; i < iterations; i++) { int wret = write(fd, write_buf, chunk_sizes[c]); - if (wret != chunk_sizes[c]) { + if (wret != (int)chunk_sizes[c]) { warn("WRITE ERROR!"); - + print_fail(); return 1; } @@ -156,6 +175,7 @@ test_mtd(int argc, char *argv[]) if (rret != chunk_sizes[c]) { warnx("READ ERROR!"); + print_fail(); return 1; } @@ -165,6 +185,7 @@ test_mtd(int argc, char *argv[]) for (int j = 0; j < chunk_sizes[c]; j++) { if (read_buf[j] != write_buf[j]) { warnx("COMPARISON ERROR: byte %d", j); + print_fail(); compare_ok = false; break; } @@ -172,6 +193,7 @@ test_mtd(int argc, char *argv[]) if (!compare_ok) { warnx("ABORTING FURTHER COMPARISON DUE TO ERROR"); + print_fail(); return 1; } @@ -183,7 +205,6 @@ test_mtd(int argc, char *argv[]) close(fd); - printf("RESULT: OK! No readback errors.\n\n"); } /* fill the file with 0xFF to make it look new again */ @@ -193,14 +214,16 @@ test_mtd(int argc, char *argv[]) for (int i = 0; i < file_size / sizeof(ffbuf); i++) { int ret = write(fd, ffbuf, sizeof(ffbuf)); - if (ret) { + if (ret != sizeof(ffbuf)) { warnx("ERROR! Could not fill file with 0xFF"); close(fd); + print_fail(); return 1; } } (void)close(fd); + print_success(); return 0; }