From d9767eb100ad6965012dfb945992843d157782d9 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 7 Nov 2013 22:23:57 +0400 Subject: 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 587b81588..09ddbf503 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 da0c11372..789a4a30d 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); + } + } } } -- cgit v1.2.3 From 08b2c338f605d4d9ffa15b151368e127ead241e3 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 7 Nov 2013 22:38:24 +0400 Subject: 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 789a4a30d..313923bcb 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 -- cgit v1.2.3 From 697df775f91ad279cb92d220a4a941f464f0628a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 8 Nov 2013 21:28:22 +0400 Subject: 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 313923bcb..2cd122ce5 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) { -- cgit v1.2.3 From 1a318ee2a60d95f7d64fe7ed13db8e5377b8c98c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 8 Nov 2013 21:29:26 +0400 Subject: 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 f94875d5b..a9c0ec6e1 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 90093a407..c4c176722 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"), -- cgit v1.2.3 From 6b085e8ced81d946307074f2ae44d6d62c63f170 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 8 Nov 2013 21:30:10 +0400 Subject: 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 ace13bb78..2499aff08 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 565b4b66a..49fe5ea4d 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 e9514446c..d0393f45a 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 40d0386d5..bdb4a0a1c 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); -- cgit v1.2.3 From e2f50f7bf880023a397957da5615d356f30f2ae4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 9 Nov 2013 17:56:40 +0400 Subject: 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 7c10e297b..5713db512 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, -- cgit v1.2.3 From 2761ea4adcc18ba326dbf24490a2f41fb0f8b9f1 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 9 Nov 2013 23:29:45 +0400 Subject: 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 c4c176722..95045134f 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"), -- cgit v1.2.3 From 75c57010d622df1bce39b27d3691337e5b11871e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 10 Nov 2013 00:06:00 +0400 Subject: 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 a9c0ec6e1..8ab4c34ef 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; -- cgit v1.2.3 From 20db1602d73b318dfc12f71fbef94a52e9073073 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 10 Nov 2013 00:12:40 +0400 Subject: 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 5713db512..20853379d 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, -- cgit v1.2.3 From e8487b7498e8a47dd93915f7ace10d97618a6969 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 10 Nov 2013 15:51:47 +0400 Subject: sensors: minor cleanup, bugs fixed, use unsigned long for discharged integration to avoid rounding errors. --- src/modules/sensors/sensors.cpp | 56 ++++++++++++++++++-------------- src/modules/uORB/topics/battery_status.h | 6 ++-- 2 files changed, 34 insertions(+), 28 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 2cd122ce5..d6231ac69 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(); } - } - - 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); + _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); + + } 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 c40d0d4e5..62796c62c 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 -- cgit v1.2.3 From 714f5ea634a184ac80254e2a415221f738d2ecd6 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 11 Nov 2013 22:02:55 +0400 Subject: 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 2499aff08..e6eaa742b 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; - - if (armed.armed) { - arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED_ERROR, &armed); + 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; - } else { - arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY_ERROR, &armed); - } + if (armed.armed) { + arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED_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 49fe5ea4d..21a1c4c2c 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 8ab4c34ef..72cfb4d0d 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 95045134f..b02f39858 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 d6231ac69..c23f29552 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 62796c62c..d473dff3f 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 */ }; -- cgit v1.2.3 From 2a2c8337e8a01c59a542c8dd3dc77a087b34e3c2 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 25 Nov 2013 19:22:06 +0400 Subject: 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 8ac2114af..f205ff8f5 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 { -- cgit v1.2.3 From 7f14f1f7deb945b7f0ba14c2f49758e9a79d12a3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 31 Dec 2013 14:45:38 +0100 Subject: 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 97410ff47..7ab1454f0 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 000000000..dc9fa23de --- /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 042322aad..e311617f9 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 000000000..0fdb170db --- /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 cac3dc82a..992a7024b 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 68a080c77..576caff99 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 000000000..50dece816 --- /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 4da86042d..d330da39f 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 72619fc8b..6a602ecfc 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 a57d04be3..321e91e60 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 1088a4407..1c717d3ea 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} }; -- cgit v1.2.3 From eb47a164cb44ddab70a2639ddf4f68519eadd4df Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 2 Jan 2014 01:18:12 +0100 Subject: 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 dc9fa23de..d791e9e2a 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 -- cgit v1.2.3 From a60fcc25358ac9ef142b456874459bec160bcbb1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 2 Jan 2014 09:18:04 +0100 Subject: 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 | 94 +++++++++++++++++------------ src/modules/systemlib/pwm_limit/pwm_limit.h | 2 +- 2 files changed, 57 insertions(+), 39 deletions(-) diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c index 992a7024b..190b315f1 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) { - - /* safeguard against overflows */ - uint16_t disarmed = disarmed_pwm[i]; - if (disarmed > min_pwm[i]) - disarmed = min_pwm[i]; - - 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]; - } + progress = diff * 10000 / RAMP_TIME_US; + + for (unsigned i=0; i 0) { + + /* safeguard against overflows */ + unsigned disarmed = disarmed_pwm[i]; + if (disarmed > min_pwm[i]) { + disarmed = min_pwm[i]; + } - 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]; + 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; + } } break; case PWM_LIMIT_STATE_ON: for (unsigned i=0; i Date: Thu, 2 Jan 2014 09:18:36 +0100 Subject: 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 d330da39f..460e6f5e7 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 -- cgit v1.2.3 From 7ae71316fa90a2da250411022a14d8e9432a8e35 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 2 Jan 2014 09:49:43 +0100 Subject: 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 d791e9e2a..01b5958b7 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 +} -- cgit v1.2.3 From 9612514a3f59cfedbd7b29c9e4f30c1edf1c7345 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 2 Jan 2014 09:50:09 +0100 Subject: 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 460e6f5e7..2a47551ee 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(); -- cgit v1.2.3 From 255d91d8d49ce06f065b6a0269bdfabeaa40fae4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 7 Jan 2014 21:56:35 +0100 Subject: hw_ver app added for hardware version checking --- ROMFS/px4fmu_common/init.d/1001_rc_quad.hil | 7 ++- ROMFS/px4fmu_common/init.d/800_sdlogger | 4 +- ROMFS/px4fmu_common/init.d/rc.io | 2 +- ROMFS/px4fmu_common/init.d/rc.logging | 4 +- ROMFS/px4fmu_common/init.d/rc.sensors | 4 +- ROMFS/px4fmu_common/init.d/rcS | 9 ++-- makefiles/config_px4fmu-v1_backside.mk | 1 + makefiles/config_px4fmu-v1_default.mk | 1 + makefiles/config_px4fmu-v2_default.mk | 1 + makefiles/config_px4fmu-v2_test.mk | 1 + src/lib/version/version.h | 62 ++++++++++++++++++++++++ src/modules/sdlog2/sdlog2.c | 2 +- src/modules/sdlog2/sdlog2_version.h | 62 ------------------------ src/systemcmds/hw_ver/hw_ver.c | 73 +++++++++++++++++++++++++++++ src/systemcmds/hw_ver/module.mk | 43 +++++++++++++++++ 15 files changed, 202 insertions(+), 74 deletions(-) create mode 100644 src/lib/version/version.h delete mode 100644 src/modules/sdlog2/sdlog2_version.h create mode 100644 src/systemcmds/hw_ver/hw_ver.c create mode 100644 src/systemcmds/hw_ver/module.mk diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil index 9b664d63e..25ea25ae8 100644 --- a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil @@ -67,8 +67,11 @@ if px4io start then echo "IO started" else - fmu mode_serial - echo "FMU started" + if hw_ver compare PX4FMU_V1 + then + fmu mode_serial + echo "FMU started" + fi fi # diff --git a/ROMFS/px4fmu_common/init.d/800_sdlogger b/ROMFS/px4fmu_common/init.d/800_sdlogger index 9b90cbdd0..2d2c3737b 100644 --- a/ROMFS/px4fmu_common/init.d/800_sdlogger +++ b/ROMFS/px4fmu_common/init.d/800_sdlogger @@ -42,10 +42,12 @@ position_estimator_inav start if [ -d /fs/microsd ] then - if [ $BOARD == fmuv1 ] + if hw_ver compare PX4FMU_V1 then + echo "Start sdlog2 at 50Hz" sdlog2 start -r 50 -e -b 16 else + echo "Start sdlog2 at 200Hz" sdlog2 start -r 200 -e -b 16 fi fi diff --git a/ROMFS/px4fmu_common/init.d/rc.io b/ROMFS/px4fmu_common/init.d/rc.io index aaf91b316..24784610c 100644 --- a/ROMFS/px4fmu_common/init.d/rc.io +++ b/ROMFS/px4fmu_common/init.d/rc.io @@ -11,7 +11,7 @@ then # # Disable px4io topic limiting # - if [ $BOARD == fmuv1 ] + if hw_ver compare PX4FMU_V1 then px4io limit 200 else diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging index dc4be8055..1791acbee 100644 --- a/ROMFS/px4fmu_common/init.d/rc.logging +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -5,10 +5,12 @@ if [ -d /fs/microsd ] then - if [ $BOARD == fmuv1 ] + if hw_ver compare PX4FMU_V1 then + echo "Start sdlog2 at 50Hz" sdlog2 start -r 50 -a -b 16 else + echo "Start sdlog2 at 200Hz" sdlog2 start -r 200 -a -b 16 fi fi diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 070a4e7e3..a2517135f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -28,9 +28,7 @@ fi if lsm303d start then - set BOARD fmuv2 -else - set BOARD fmuv1 + echo "using LSM303D" fi # Start airspeed sensors diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 66cb3f237..8801d1126 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -185,9 +185,12 @@ then else echo "PX4IO restart failed" echo "PX4IO restart failed" >> $logfile - tone_alarm MNGGG - sleep 10 - reboot + if hw_ver compare PX4FMU_V2 + then + tone_alarm MNGGG + sleep 10 + reboot + fi fi else echo "PX4IO update failed" diff --git a/makefiles/config_px4fmu-v1_backside.mk b/makefiles/config_px4fmu-v1_backside.mk index c86beacca..a37aa8f96 100644 --- a/makefiles/config_px4fmu-v1_backside.mk +++ b/makefiles/config_px4fmu-v1_backside.mk @@ -54,6 +54,7 @@ MODULES += systemcmds/top MODULES += systemcmds/tests MODULES += systemcmds/config MODULES += systemcmds/nshterm +MODULES += systemcmds/hw_ver # # General system control diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index d0733ec53..33c0b99e2 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -58,6 +58,7 @@ MODULES += systemcmds/top MODULES += systemcmds/tests MODULES += systemcmds/config MODULES += systemcmds/nshterm +MODULES += systemcmds/hw_ver # # General system control diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index afe072b64..475cbae01 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/hw_ver # # General system control diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index 0f60e88b5..750c0e7c3 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -16,6 +16,7 @@ MODULES += drivers/led MODULES += drivers/boards/px4fmu-v2 MODULES += systemcmds/perf MODULES += systemcmds/reboot +MODULES += systemcmds/hw_ver # # Library modules diff --git a/src/lib/version/version.h b/src/lib/version/version.h new file mode 100644 index 000000000..af733aaf0 --- /dev/null +++ b/src/lib/version/version.h @@ -0,0 +1,62 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin + * + * 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 version.h + * + * Tools for system version detection. + * + * @author Anton Babushkin + */ + +#ifndef VERSION_H_ +#define VERSION_H_ + +/* + GIT_VERSION is defined at build time via a Makefile call to the + git command line. + */ +#define FREEZE_STR(s) #s +#define STRINGIFY(s) FREEZE_STR(s) +#define FW_GIT STRINGIFY(GIT_VERSION) + +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 +#define HW_ARCH "PX4FMU_V1" +#endif + +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 +#define HW_ARCH "PX4FMU_V2" +#endif + +#endif /* VERSION_H_ */ diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index e94b1e13c..658c6779c 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -85,13 +85,13 @@ #include #include +#include #include #include "logbuffer.h" #include "sdlog2_format.h" #include "sdlog2_messages.h" -#include "sdlog2_version.h" #define LOGBUFFER_WRITE_AND_COUNT(_msg) if (logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(_msg))) { \ log_msgs_written++; \ diff --git a/src/modules/sdlog2/sdlog2_version.h b/src/modules/sdlog2/sdlog2_version.h deleted file mode 100644 index c6a9ba638..000000000 --- a/src/modules/sdlog2/sdlog2_version.h +++ /dev/null @@ -1,62 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin - * - * 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 sdlog2_version.h - * - * Tools for system version detection. - * - * @author Anton Babushkin - */ - -#ifndef SDLOG2_VERSION_H_ -#define SDLOG2_VERSION_H_ - -/* - GIT_VERSION is defined at build time via a Makefile call to the - git command line. - */ -#define FREEZE_STR(s) #s -#define STRINGIFY(s) FREEZE_STR(s) -#define FW_GIT STRINGIFY(GIT_VERSION) - -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 -#define HW_ARCH "PX4FMU_V1" -#endif - -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 -#define HW_ARCH "PX4FMU_V2" -#endif - -#endif /* SDLOG2_VERSION_H_ */ diff --git a/src/systemcmds/hw_ver/hw_ver.c b/src/systemcmds/hw_ver/hw_ver.c new file mode 100644 index 000000000..4b84523cc --- /dev/null +++ b/src/systemcmds/hw_ver/hw_ver.c @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * Copyright (c) 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 hw_ver.c + * + * Show and test hardware version. + */ + +#include + +#include +#include +#include +#include + +__EXPORT int hw_ver_main(int argc, char *argv[]); + +int +hw_ver_main(int argc, char *argv[]) +{ + if (argc >= 2) { + if (!strcmp(argv[1], "show")) { + printf(HW_ARCH "\n"); + exit(0); + } + + if (!strcmp(argv[1], "compare")) { + if (argc >= 3) { + int ret = strcmp(HW_ARCH, argv[2]) != 0; + if (ret == 0) { + printf("hw_ver match: %s\n", HW_ARCH); + } + exit(ret); + + } else { + errx(1, "not enough arguments, try 'compare PX4FMU_1'"); + } + } + } + + errx(1, "expected a command, try 'show' or 'compare'"); +} diff --git a/src/systemcmds/hw_ver/module.mk b/src/systemcmds/hw_ver/module.mk new file mode 100644 index 000000000..3cc08b6a1 --- /dev/null +++ b/src/systemcmds/hw_ver/module.mk @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 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. +# +############################################################################ + +# +# Show and test hardware version +# + +MODULE_COMMAND = hw_ver +SRCS = hw_ver.c + +MODULE_STACKSIZE = 1024 + +MAXOPTIMIZATION = -Os -- cgit v1.2.3 From 4cffd99db940a9f0cda7643842ccf17d8a3f1b48 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 8 Jan 2014 20:55:12 +0100 Subject: Major autostart rewrite --- ROMFS/px4fmu_common/init.d/1001_rc_quad.hil | 138 +++------- ROMFS/px4fmu_common/init.d/4010_dji_f330 | 67 ++--- ROMFS/px4fmu_common/init.d/4011_dji_f450 | 66 +---- ROMFS/px4fmu_common/init.d/rc.autostart | 180 +++++++++++++ ROMFS/px4fmu_common/init.d/rc.autostart_hil | 34 +++ ROMFS/px4fmu_common/init.d/rc.io | 32 +-- ROMFS/px4fmu_common/init.d/rc.mc_apps | 24 ++ ROMFS/px4fmu_common/init.d/rc.mc_interface | 23 +- ROMFS/px4fmu_common/init.d/rc.multirotor | 39 --- ROMFS/px4fmu_common/init.d/rcS | 383 ++++++++++------------------ 10 files changed, 477 insertions(+), 509 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/rc.autostart create mode 100644 ROMFS/px4fmu_common/init.d/rc.autostart_hil create mode 100644 ROMFS/px4fmu_common/init.d/rc.mc_apps delete mode 100644 ROMFS/px4fmu_common/init.d/rc.multirotor diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil index 25ea25ae8..8732673f7 100644 --- a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil @@ -1,108 +1,42 @@ #!nsh -# -# USB HIL start -# -echo "[HIL] HILS quadrotor starting.." +echo "[init] PX4FMU v1, v2 with or without IO on HIL Quad" -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - - param set MC_ATTRATE_D 0.0 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.05 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 - param set MC_ATT_P 3.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_P 2.1 - param set MC_YAWRATE_D 0.0 - param set MC_YAWRATE_I 0.0 - param set MC_YAWRATE_P 0.05 - param set NAV_TAKEOFF_ALT 3.0 - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.5 - param set MPC_THR_MIN 0.1 - param set MPC_XY_D 0 - param set MPC_XY_P 0.5 - param set MPC_XY_VEL_D 0 - param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 3 - param set MPC_XY_VEL_P 0.2 - param set MPC_Z_D 0 - param set MPC_Z_P 1 - param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.1 - param set MPC_Z_VEL_MAX 2 - param set MPC_Z_VEL_P 0.20 - - param save -fi - -# Allow USB some time to come up -sleep 1 -# Tell MAVLink that this link is "fast" -mavlink start -b 230400 -d /dev/ttyACM0 - -# Create a fake HIL /dev/pwm_output interface -hil mode_pwm - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ -# -param set MAV_TYPE 2 - -# -# Check if we got an IO -# -if px4io start -then - echo "IO started" -else - if hw_ver compare PX4FMU_V1 - then - fmu mode_serial - echo "FMU started" - fi + # + # Default parameters for this platform + # + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_YAWPOS_P 2.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + param set MPC_TILT_MAX 0.5 + param set MPC_THR_MAX 0.8 + param set MPC_THR_MIN 0.2 + param set MPC_XY_D 0 + param set MPC_XY_P 0.5 + param set MPC_XY_VEL_D 0 + param set MPC_XY_VEL_I 0 + param set MPC_XY_VEL_MAX 3 + param set MPC_XY_VEL_P 0.2 + param set MPC_Z_D 0 + param set MPC_Z_P 1 + param set MPC_Z_VEL_D 0 + param set MPC_Z_VEL_I 0.1 + param set MPC_Z_VEL_MAX 2 + param set MPC_Z_VEL_P 0.20 fi -# -# Start the sensors (depends on orb, px4io) -# -sh /etc/init.d/rc.sensors - -# -# Start the attitude estimator (depends on orb) -# -att_pos_estimator_ekf start - -# -# Load mixer and start controllers (depends on px4io) -# -mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix - -# -# Start position estimator -# -position_estimator_inav start - -# -# Start attitude control -# -multirotor_att_control start - -# -# Start position control -# -multirotor_pos_control start - -echo "[HIL] setup done, running" - +set FRAME_TYPE mc +set FRAME_GEOMETRY x +set FRAME_COUNT 4 diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index 7054210e2..58c6d99bb 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -2,30 +2,27 @@ echo "[init] PX4FMU v1, v2 with or without IO on DJI F330" -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - - param set MC_ATTRATE_D 0.004 - param set MC_ATTRATE_I 0.0 + # + # Default parameters for this platform + # param set MC_ATTRATE_P 0.12 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 param set MC_ATT_P 7.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 param set MC_YAWPOS_P 2.0 - param set MC_YAWRATE_D 0.005 - param set MC_YAWRATE_I 0.2 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 param set MC_YAWRATE_P 0.3 - param set NAV_TAKEOFF_ALT 3.0 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.7 - param set MPC_THR_MIN 0.3 + param set MPC_THR_MAX 0.8 + param set MPC_THR_MIN 0.2 param set MPC_XY_D 0 param set MPC_XY_P 0.5 param set MPC_XY_VEL_D 0 @@ -38,32 +35,12 @@ then param set MPC_Z_VEL_I 0.1 param set MPC_Z_VEL_MAX 2 param set MPC_Z_VEL_P 0.20 - - param save fi -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - usleep 5000 - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -sh /etc/init.d/rc.mc_interface - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor +set FRAME_TYPE mc +set FRAME_GEOMETRY x +set FRAME_COUNT 4 +set PWM_RATE 400 +set PWM_MIN 1200 +set PWM_MAX 1900 +set PWM_DISARMED 900 diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index a1d253191..ca055dfcb 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -2,14 +2,11 @@ echo "[init] PX4FMU v1, v2 with or without IO on DJI F450" -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - + # + # Default parameters for this platform + # param set MC_ATTRATE_D 0.004 param set MC_ATTRATE_I 0.0 param set MC_ATTRATE_P 0.12 @@ -22,53 +19,12 @@ then param set MC_YAWRATE_D 0.005 param set MC_YAWRATE_I 0.2 param set MC_YAWRATE_P 0.3 - - param save fi -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - usleep 5000 - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer -# -mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix - -# -# Set PWM output frequency -# -pwm rate -c 1234 -r 400 - -# -# Set disarmed, min and max PWM signals (for DJI ESCs) -# -pwm disarmed -c 1234 -p 900 -pwm min -c 1234 -p 1200 -pwm max -c 1234 -p 1800 - -# -# Start common multirotor apps -# -sh /etc/init.d/rc.multirotor +set FRAME_TYPE mc +set FRAME_GEOMETRY x +set FRAME_COUNT 4 +set PWM_RATE 400 +set PWM_MIN 1200 +set PWM_MAX 1900 +set PWM_DISARMED 900 diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart new file mode 100644 index 000000000..10b7bc424 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -0,0 +1,180 @@ +# +# Check if auto-setup from one of the standard scripts is wanted +# SYS_AUTOSTART = 0 means no autostart (default) +# +# AUTOSTART PARTITION: +# 0 .. 999 Reserved (historical) +# 1000 .. 1999 Simulation setups +# 2000 .. 2999 Standard planes +# 3000 .. 3999 Flying wing +# 4000 .. 4999 Quad X +# 5000 .. 5999 Quad + +# 6000 .. 6999 Hexa X +# 7000 .. 7999 Hexa + +# 8000 .. 8999 Octo X +# 9000 .. 9999 Octo + +# 10000 .. 10999 Wide arm / H frame +# 11000 .. 11999 Hexa Cox +# 12000 .. 12999 Octo Cox + +if param compare SYS_AUTOSTART 4008 8 +then + #sh /etc/init.d/4008_ardrone +fi + +if param compare SYS_AUTOSTART 4009 9 +then + #sh /etc/init.d/4009_ardrone_flow +fi + +if param compare SYS_AUTOSTART 4010 10 +then + sh /etc/init.d/4010_dji_f330 +fi + +if param compare SYS_AUTOSTART 4011 11 +then + sh /etc/init.d/4011_dji_f450 +fi + +if param compare SYS_AUTOSTART 4012 +then + #sh /etc/init.d/666_fmu_q_x550 +fi + +if param compare SYS_AUTOSTART 6012 12 +then + #set MIXER /etc/mixers/FMU_hex_x.mix + #sh /etc/init.d/rc.hexa +fi + +if param compare SYS_AUTOSTART 7013 13 +then + #set MIXER /etc/mixers/FMU_hex_+.mix + #sh /etc/init.d/rc.hexa +fi + +if param compare SYS_AUTOSTART 8001 +then + #set MIXER /etc/mixers/FMU_octo_x.mix + #sh /etc/init.d/rc.octo +fi + +if param compare SYS_AUTOSTART 9001 +then + #set MIXER /etc/mixers/FMU_octo_+.mix + #sh /etc/init.d/rc.octo +fi + +if param compare SYS_AUTOSTART 12001 +then + #set MIXER /etc/mixers/FMU_octo_cox.mix + #sh /etc/init.d/rc.octo +fi + +if param compare SYS_AUTOSTART 10015 15 +then + #sh /etc/init.d/10015_tbs_discovery +fi + +if param compare SYS_AUTOSTART 10016 16 +then + #sh /etc/init.d/10016_3dr_iris +fi + +if param compare SYS_AUTOSTART 4017 17 +then + #set MKBLCTRL_MODE no + #set MKBLCTRL_FRAME x + #sh /etc/init.d/rc.custom_dji_f330_mkblctrl +fi + +if param compare SYS_AUTOSTART 5018 18 +then + #set MKBLCTRL_MODE no + #set MKBLCTRL_FRAME + + #sh /etc/init.d/rc.custom_dji_f330_mkblctrl +fi + +if param compare SYS_AUTOSTART 4019 19 +then + #set MKBLCTRL_MODE yes + #set MKBLCTRL_FRAME x + #sh /etc/init.d/rc.custom_dji_f330_mkblctrl +fi + +if param compare SYS_AUTOSTART 5020 20 +then + #set MKBLCTRL_MODE yes + #set MKBLCTRL_FRAME + + #sh /etc/init.d/rc.custom_dji_f330_mkblctrl +fi + +if param compare SYS_AUTOSTART 4021 21 +then + #set FRAME_GEOMETRY x + #set ESC_MAKER afro + #sh /etc/init.d/rc.custom_io_esc +fi + +if param compare SYS_AUTOSTART 10022 22 +then + #set FRAME_GEOMETRY w + #sh /etc/init.d/rc.custom_io_esc +fi + +if param compare SYS_AUTOSTART 3030 30 +then + #sh /etc/init.d/3030_io_camflyer +fi + +if param compare SYS_AUTOSTART 3031 31 +then + #sh /etc/init.d/3031_io_phantom +fi + +if param compare SYS_AUTOSTART 3032 32 +then + #sh /etc/init.d/3032_skywalker_x5 +fi + +if param compare SYS_AUTOSTART 3033 33 +then + #sh /etc/init.d/3033_io_wingwing +fi + +if param compare SYS_AUTOSTART 3034 34 +then + #sh /etc/init.d/3034_io_fx79 + #set MODE custom +fi + +if param compare SYS_AUTOSTART 40 +then + #sh /etc/init.d/40_io_segway + #set MODE custom +fi + +if param compare SYS_AUTOSTART 2100 100 +then + #sh /etc/init.d/2100_mpx_easystar + #set MODE custom +fi + +if param compare SYS_AUTOSTART 2101 101 +then + #sh /etc/init.d/2101_hk_bixler + #set MODE custom +fi + +if param compare SYS_AUTOSTART 2102 102 +then + #sh /etc/init.d/2102_3dr_skywalker + #set MODE custom +fi + +if param compare SYS_AUTOSTART 800 +then + #sh /etc/init.d/800_sdlogger + #set MODE custom +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart_hil b/ROMFS/px4fmu_common/init.d/rc.autostart_hil new file mode 100644 index 000000000..d5fc5eb08 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.autostart_hil @@ -0,0 +1,34 @@ +# +# Check if auto-setup from one of the standard scripts for HIL is wanted +# + +if param compare SYS_AUTOSTART 1000 +then + #sh /etc/init.d/1000_rc_fw_easystar.hil + set MODE hil +fi + +if param compare SYS_AUTOSTART 1001 +then + sh /etc/init.d/1001_rc_quad.hil + set MODE hil +fi + +if param compare SYS_AUTOSTART 1002 +then + #sh /etc/init.d/1002_rc_fw_state.hil + set MODE hil +fi + +if param compare SYS_AUTOSTART 1003 +then + #sh /etc/init.d/1003_rc_quad_+.hil + set MODE hil +fi + +if param compare SYS_AUTOSTART 1004 +then + #sh /etc/init.d/1004_rc_fw_Rascal110.hil + set MODE hil +fi + diff --git a/ROMFS/px4fmu_common/init.d/rc.io b/ROMFS/px4fmu_common/init.d/rc.io index 24784610c..5a010cc9b 100644 --- a/ROMFS/px4fmu_common/init.d/rc.io +++ b/ROMFS/px4fmu_common/init.d/rc.io @@ -1,23 +1,19 @@ # -# Start PX4IO interface (depends on orb, commander) +# Init PX4IO interface # -if px4io start -then - # - # Allow PX4IO to recover from midair restarts. - # this is very unlikely, but quite safe and robust. - px4io recovery - # - # Disable px4io topic limiting - # - if hw_ver compare PX4FMU_V1 - then - px4io limit 200 - else - px4io limit 400 - fi +# +# Allow PX4IO to recover from midair restarts. +# this is very unlikely, but quite safe and robust. +# +px4io recovery + +# +# Adjust px4io topic limiting +# +if hw_ver compare PX4FMU_V1 +then + px4io limit 200 else - # SOS - tone_alarm error + px4io limit 400 fi diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps new file mode 100644 index 000000000..16a7a33c6 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -0,0 +1,24 @@ +#!nsh +# +# Standard everything needed for multirotors except mixer, actuator output and mavlink +# + +# +# Start the attitude estimator +# +attitude_estimator_ekf start + +# +# Start position estimator +# +position_estimator_inav start + +# +# Start attitude control +# +multirotor_att_control start + +# +# Start position control +# +multirotor_pos_control start diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_interface b/ROMFS/px4fmu_common/init.d/rc.mc_interface index 6bb2e84ec..2a05012a6 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_interface +++ b/ROMFS/px4fmu_common/init.d/rc.mc_interface @@ -35,15 +35,26 @@ else fi fi - # # Set PWM output frequency # -pwm rate -c $OUTPUTS -r $PWM_RATE +if [ $PWM_RATE != none ] +then + pwm rate -c $OUTPUTS -r $PWM_RATE +fi # -# Set disarmed, min and max PWM signals (for DJI ESCs) +# Set disarmed, min and max PWM values # -pwm disarmed -c $OUTPUTS -p $PWM_DISARMED -pwm min -c $OUTPUTS -p $PWM_MIN -pwm max -c $OUTPUTS -p $PWM_MAX +if [ $PWM_DISARMED != none ] +then + pwm disarmed -c $OUTPUTS -p $PWM_DISARMED +fi +if [ $PWM_MIN != none ] +then + pwm min -c $OUTPUTS -p $PWM_MIN +fi +if [ $PWM_MAX != none ] +then + pwm max -c $OUTPUTS -p $PWM_MAX +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.multirotor b/ROMFS/px4fmu_common/init.d/rc.multirotor deleted file mode 100644 index bc550ac5a..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.multirotor +++ /dev/null @@ -1,39 +0,0 @@ -#!nsh -# -# Standard everything needed for multirotors except mixer, actuator output and mavlink -# - -# -# Start the sensors and test them. -# -sh /etc/init.d/rc.sensors - -# -# Start logging (depends on sensors) -# -sh /etc/init.d/rc.logging - -# -# Start GPS interface (depends on orb) -# -gps start - -# -# Start the attitude estimator -# -attitude_estimator_ekf start - -# -# Start position estimator -# -position_estimator_inav start - -# -# Start attitude control -# -multirotor_att_control start - -# -# Start position control -# -multirotor_pos_control start diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 8801d1126..4dd9af407 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -60,12 +60,9 @@ fi if [ $MODE == autostart ] then # - # Start terminal + # Start CDC/ACM serial driver # - if sercon - then - echo "USB connected" - fi + sercon # # Start the ORB (first app to start) @@ -107,52 +104,79 @@ then blinkm systemstate fi fi + + set USE_IO no + set FRAME_TYPE none + set PWM_RATE none + set PWM_DISARMED none + set PWM_MIN none + set PWM_MAX none + + if param compare SYS_AUTOCONFIG 1 + then + set DO_AUTOCONFIG yes + else + set DO_AUTOCONFIG no + fi # # Start the Commander (needs to be this early for in-air-restarts) # commander start - - if param compare SYS_AUTOSTART 1000 - then - sh /etc/init.d/1000_rc_fw_easystar.hil - set MODE custom - fi - - if param compare SYS_AUTOSTART 1001 - then - sh /etc/init.d/1001_rc_quad.hil - set MODE custom - fi - if param compare SYS_AUTOSTART 1002 - then - sh /etc/init.d/1002_rc_fw_state.hil - set MODE custom - fi + # + # Set parameters and env variables for selected AUTOSTART (HIL setups) + # + sh /etc/init.d/rc.autostart_hil - if param compare SYS_AUTOSTART 1003 - then - sh /etc/init.d/1003_rc_quad_+.hil - set MODE custom - fi - - if param compare SYS_AUTOSTART 1004 - then - sh /etc/init.d/1004_rc_fw_Rascal110.hil - set MODE custom - fi + if [ $MODE == hil ] + then + # + # Do common HIL setup depending on env variables + # + # Allow USB some time to come up + sleep 1 + + # Start MAVLink on USB port + mavlink start -b 230400 -d /dev/ttyACM0 + usleep 5000 + + # Create a fake HIL /dev/pwm_output interface + hil mode_pwm + + # Sensors + echo "Start sensors" + sh /etc/init.d/rc.sensors + + # + # Fixed wing setup + # + if [ $FRAME_TYPE == fw ] + then + echo "Setup FIXED WING" + fi - if [ $MODE != custom ] - then - # Try to get an USB console + # + # Multicopters setup + # + if [ $FRAME_TYPE == mc ] + then + echo "Setup MULTICOPTER" + + # Load mixer and configure outputs + sh /etc/init.d/rc.mc_interface + + # Start common multicopter apps + sh /etc/init.d/rc.mc_apps + fi + else + # Try to get an USB console if not in HIL mode nshterm /dev/ttyACM0 & fi - + # # Upgrade PX4IO firmware # - if [ -f /etc/extras/px4io-v2_default.bin ] then set io_file /etc/extras/px4io-v2_default.bin @@ -170,6 +194,7 @@ then then echo "PX4IO CRC OK" echo "PX4IO CRC OK" >> $logfile + set USE_IO yes else echo "PX4IO CRC failure" echo "PX4IO CRC failure" >> $logfile @@ -182,6 +207,7 @@ then echo "PX4IO restart OK" echo "PX4IO restart OK" >> $logfile tone_alarm MSPAA + set USE_IO yes else echo "PX4IO restart failed" echo "PX4IO restart failed" >> $logfile @@ -200,222 +226,91 @@ then fi set EXIT_ON_END no + set USE_LOGGING yes + set USE_GPS yes # - # Check if auto-setup from one of the standard scripts is wanted - # SYS_AUTOSTART = 0 means no autostart (default) + # Set parameters and env variables for selected AUTOSTART # - # AUTOSTART PARTITION: - # 0 .. 999 Reserved (historical) - # 1000 .. 1999 Simulation setups - # 2000 .. 2999 Standard planes - # 3000 .. 3999 Flying wing - # 4000 .. 4999 Quad X - # 5000 .. 5999 Quad + - # 6000 .. 6999 Hexa X - # 7000 .. 7999 Hexa + - # 8000 .. 8999 Octo X - # 9000 .. 9999 Octo + - # 10000 .. 10999 Wide arm / H frame - # 11000 .. 11999 Hexa Cox - # 12000 .. 12999 Octo Cox - - if param compare SYS_AUTOSTART 4008 8 - then - sh /etc/init.d/4008_ardrone - set MODE custom - fi - - if param compare SYS_AUTOSTART 4009 9 - then - sh /etc/init.d/4009_ardrone_flow - set MODE custom - fi - - if param compare SYS_AUTOSTART 4010 10 - then - set FRAME_GEOMETRY x - set FRAME_COUNT 4 - set PWM_MIN 1200 - set PWM_MAX 1900 - set PWM_DISARMED 900 - sh /etc/init.d/4010_dji_f330 - set MODE custom - fi - - if param compare SYS_AUTOSTART 4011 11 - then - sh /etc/init.d/4011_dji_f450 - set MODE custom - fi - - if param compare SYS_AUTOSTART 4012 - then - sh /etc/init.d/666_fmu_q_x550 - set MODE custom - fi - - if param compare SYS_AUTOSTART 6012 12 - then - set MIXER /etc/mixers/FMU_hex_x.mix - sh /etc/init.d/rc.hexa - set MODE custom - fi - - if param compare SYS_AUTOSTART 7013 13 - then - set MIXER /etc/mixers/FMU_hex_+.mix - sh /etc/init.d/rc.hexa - set MODE custom - fi - - if param compare SYS_AUTOSTART 8001 - then - set MIXER /etc/mixers/FMU_octo_x.mix - sh /etc/init.d/rc.octo - set MODE custom - fi - - if param compare SYS_AUTOSTART 9001 - then - set MIXER /etc/mixers/FMU_octo_+.mix - sh /etc/init.d/rc.octo - set MODE custom - fi - - if param compare SYS_AUTOSTART 12001 - then - set MIXER /etc/mixers/FMU_octo_cox.mix - sh /etc/init.d/rc.octo - set MODE custom - fi + sh /etc/init.d/rc.autostart - if param compare SYS_AUTOSTART 10015 15 - then - sh /etc/init.d/10015_tbs_discovery - set MODE custom - fi - - if param compare SYS_AUTOSTART 10016 16 - then - sh /etc/init.d/10016_3dr_iris - set MODE custom - fi - - # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame - if param compare SYS_AUTOSTART 4017 17 - then - set MKBLCTRL_MODE no - set MKBLCTRL_FRAME x - sh /etc/init.d/rc.custom_dji_f330_mkblctrl - set MODE custom - fi - - # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame - if param compare SYS_AUTOSTART 5018 18 - then - set MKBLCTRL_MODE no - set MKBLCTRL_FRAME + - sh /etc/init.d/rc.custom_dji_f330_mkblctrl - set MODE custom - fi - - # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame Mikrokopter-Addressing - if param compare SYS_AUTOSTART 4019 19 - then - set MKBLCTRL_MODE yes - set MKBLCTRL_FRAME x - sh /etc/init.d/rc.custom_dji_f330_mkblctrl - set MODE custom - fi - - # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame Mikrokopter-Addressing - if param compare SYS_AUTOSTART 5020 20 - then - set MKBLCTRL_MODE yes - set MKBLCTRL_FRAME + - sh /etc/init.d/rc.custom_dji_f330_mkblctrl - set MODE custom - fi - - # PX4FMU v1 with IO + PPM-based ESCs on Quad X-shape frame - if param compare SYS_AUTOSTART 4021 21 - then - set FRAME_GEOMETRY x - set ESC_MAKER afro - sh /etc/init.d/rc.custom_io_esc - set MODE custom - fi - - # PX4FMU v1 with IO + PPM-based ESCs on Quad X-shape frame - if param compare SYS_AUTOSTART 10022 22 - then - set FRAME_GEOMETRY w - sh /etc/init.d/rc.custom_io_esc - set MODE custom - fi - - if param compare SYS_AUTOSTART 3030 30 - then - sh /etc/init.d/3030_io_camflyer - set MODE custom - fi - - if param compare SYS_AUTOSTART 3031 31 + # + # If autoconfig parameter was set, reset it and save parameters + # + if [ $DO_AUTOCONFIG == yes ] then - sh /etc/init.d/3031_io_phantom - set MODE custom + param set SYS_AUTOCONFIG 0 + param save fi - if param compare SYS_AUTOSTART 3032 32 - then - sh /etc/init.d/3032_skywalker_x5 - set MODE custom - fi - - if param compare SYS_AUTOSTART 3033 33 - then - sh /etc/init.d/3033_io_wingwing - set MODE custom - fi - - if param compare SYS_AUTOSTART 3034 34 - then - sh /etc/init.d/3034_io_fx79 - set MODE custom - fi - - if param compare SYS_AUTOSTART 40 - then - sh /etc/init.d/40_io_segway - set MODE custom - fi + if [ $MODE == autostart ] + then + # + # Do common setup depending on env variables + # + if [ $USE_IO == yes ] + then + echo "Use IO" + + # Start MAVLink on default port: ttyS1 + mavlink start + usleep 5000 + + sh /etc/init.d/rc.io + else + echo "Don't use IO" + + # Start MAVLink on ttyS0 + mavlink start -d /dev/ttyS0 + usleep 5000 + + # Configure FMU for PWM outputs + fmu mode_pwm + + # Exit from nsh to free port for mavlink + set EXIT_ON_END yes + fi + + # Sensors + echo "Start sensors" + sh /etc/init.d/rc.sensors + + # Logging + if [ $USE_LOGGING == yes ] + then + sh /etc/init.d/rc.logging + fi + + # GPS interface + if [ $USE_GPS == yes ] + then + gps start + fi + + # + # Fixed wing setup + # + if [ $FRAME_TYPE == fw ] + then + echo "Setup FIXED WING" + fi - if param compare SYS_AUTOSTART 2100 100 - then - sh /etc/init.d/2100_mpx_easystar - set MODE custom - fi - - if param compare SYS_AUTOSTART 2101 101 - then - sh /etc/init.d/2101_hk_bixler - set MODE custom + # + # Multicopters setup + # + if [ $FRAME_TYPE == mc ] + then + echo "Setup MULTICOPTER" + + # Load mixer and configure outputs + sh /etc/init.d/rc.mc_interface + + # Start common multicopter apps + sh /etc/init.d/rc.mc_apps + fi fi - if param compare SYS_AUTOSTART 2102 102 - then - sh /etc/init.d/2102_3dr_skywalker - set MODE custom - fi - - if param compare SYS_AUTOSTART 800 - then - sh /etc/init.d/800_sdlogger - set MODE custom - fi - - # Start any custom extensions that might be missing + # Start any custom extensions if [ -f /fs/microsd/etc/rc.local ] then sh /fs/microsd/etc/rc.local -- cgit v1.2.3 From 9a5ef700709b50d57201e77bc80f11c47b25f548 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 8 Jan 2014 23:31:49 +0100 Subject: init: USE_LOGGING and USE_GPS env vars removed --- ROMFS/px4fmu_common/init.d/rcS | 12 ++---------- 1 file changed, 2 insertions(+), 10 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 4dd9af407..7d38736de 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -226,8 +226,6 @@ then fi set EXIT_ON_END no - set USE_LOGGING yes - set USE_GPS yes # # Set parameters and env variables for selected AUTOSTART @@ -276,16 +274,10 @@ then sh /etc/init.d/rc.sensors # Logging - if [ $USE_LOGGING == yes ] - then - sh /etc/init.d/rc.logging - fi + sh /etc/init.d/rc.logging # GPS interface - if [ $USE_GPS == yes ] - then - gps start - fi + gps start # # Fixed wing setup -- cgit v1.2.3 From 532c4c771e3da9d0b371101a056c29d0f417cd09 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 9 Jan 2014 10:05:24 +0100 Subject: Autostart: generic quad, hexa and octo added, WIP --- ROMFS/px4fmu_common/init.d/4001_quad_x_pwm | 17 ++++++ ROMFS/px4fmu_common/init.d/4010_dji_f330 | 26 +++++---- ROMFS/px4fmu_common/init.d/4011_dji_f450 | 32 +++++----- ROMFS/px4fmu_common/init.d/4012_hk_x550 | 27 +++++++++ ROMFS/px4fmu_common/init.d/5001_quad_+_pwm | 17 ++++++ ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm | 17 ++++++ ROMFS/px4fmu_common/init.d/666_fmu_q_x550 | 76 ------------------------ ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm | 17 ++++++ ROMFS/px4fmu_common/init.d/8001_octo_x_pwm | 17 ++++++ ROMFS/px4fmu_common/init.d/9001_octo_+_pwm | 17 ++++++ ROMFS/px4fmu_common/init.d/rc.autostart | 31 +++++----- ROMFS/px4fmu_common/init.d/rc.hexa | 94 ------------------------------ ROMFS/px4fmu_common/init.d/rc.mc_interface | 42 ++++++------- 13 files changed, 196 insertions(+), 234 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/4001_quad_x_pwm create mode 100644 ROMFS/px4fmu_common/init.d/4012_hk_x550 create mode 100644 ROMFS/px4fmu_common/init.d/5001_quad_+_pwm create mode 100644 ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm delete mode 100644 ROMFS/px4fmu_common/init.d/666_fmu_q_x550 create mode 100644 ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm create mode 100644 ROMFS/px4fmu_common/init.d/8001_octo_x_pwm create mode 100644 ROMFS/px4fmu_common/init.d/9001_octo_+_pwm delete mode 100644 ROMFS/px4fmu_common/init.d/rc.hexa diff --git a/ROMFS/px4fmu_common/init.d/4001_quad_x_pwm b/ROMFS/px4fmu_common/init.d/4001_quad_x_pwm new file mode 100644 index 000000000..9bee414dc --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/4001_quad_x_pwm @@ -0,0 +1,17 @@ +#!nsh + +# Maintainers: Anton Babushkin , Lorenz Meier + +echo "[init] PX4FMU v1, v2 with or without IO on generic quadcopter X with PWM ESCs" + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # +fi + +set FRAME_TYPE mc +set FRAME_GEOMETRY x +set FRAME_COUNT 4 +set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index 58c6d99bb..93edb0005 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -1,24 +1,29 @@ #!nsh +# Maintainers: Anton Babushkin + echo "[init] PX4FMU v1, v2 with or without IO on DJI F330" +# Use generic quad X PWM as base +sh /etc/init.d/4001_quad_x_pwm + if [ $DO_AUTOCONFIG == yes ] then # # Default parameters for this platform # - param set MC_ATTRATE_P 0.12 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_D 0.004 param set MC_ATT_P 7.0 param set MC_ATT_I 0.0 param set MC_ATT_D 0.0 - param set MC_YAWPOS_P 2.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_YAWPOS_P 2.8 param set MC_YAWPOS_I 0.0 param set MC_YAWPOS_D 0.0 - param set MC_YAWRATE_P 0.3 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_D 0.005 + param set MC_YAWRATE_P 0.2 + param set MC_YAWRATE_I 0.05 + param set MC_YAWRATE_D 0.0 param set MPC_TILT_MAX 0.5 param set MPC_THR_MAX 0.8 @@ -37,10 +42,7 @@ then param set MPC_Z_VEL_P 0.20 fi -set FRAME_TYPE mc -set FRAME_GEOMETRY x -set FRAME_COUNT 4 -set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 set PWM_MIN 1200 set PWM_MAX 1900 -set PWM_DISARMED 900 diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index ca055dfcb..cc0ddccc8 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -1,30 +1,34 @@ #!nsh +# Maintainers: Lorenz Meier + echo "[init] PX4FMU v1, v2 with or without IO on DJI F450" +# Use generic quad X PWM as base +sh /etc/init.d/4001_quad_x_pwm + if [ $DO_AUTOCONFIG == yes ] then # # Default parameters for this platform # - param set MC_ATTRATE_D 0.004 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.12 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 param set MC_ATT_P 7.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 param set MC_YAWPOS_P 2.0 - param set MC_YAWRATE_D 0.005 - param set MC_YAWRATE_I 0.2 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + # TODO add default MPC parameters fi -set FRAME_TYPE mc -set FRAME_GEOMETRY x -set FRAME_COUNT 4 -set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 set PWM_MIN 1200 set PWM_MAX 1900 -set PWM_DISARMED 900 diff --git a/ROMFS/px4fmu_common/init.d/4012_hk_x550 b/ROMFS/px4fmu_common/init.d/4012_hk_x550 new file mode 100644 index 000000000..a749b7699 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/4012_hk_x550 @@ -0,0 +1,27 @@ +#!nsh + +# Maintainers: Todd Stellanova + +echo "[init] PX4FMU v1, v2 with or without IO on HobbyKing X550" + +# Use generic quad X PWM as base +sh /etc/init.d/4001_quad_x_pwm + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ATT_P 5.5 + param set MC_ATT_I 0 + param set MC_ATT_D 0 + param set MC_ATTRATE_P 0.14 + param set MC_ATTRATE_I 0 + param set MC_ATTRATE_D 0.006 + param set MC_YAWPOS_P 0.6 + param set MC_YAWPOS_I 0 + param set MC_YAWPOS_D 0 + param set MC_YAWRATE_P 0.08 + param set MC_YAWRATE_I 0 + param set MC_YAWRATE_D 0 +fi diff --git a/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm b/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm new file mode 100644 index 000000000..7d0748668 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm @@ -0,0 +1,17 @@ +#!nsh + +# Maintainers: Anton Babushkin , Lorenz Meier + +echo "[init] PX4FMU v1, v2 with or without IO on generic quadcopter + with PWM ESCs" + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # +fi + +set FRAME_TYPE mc +set FRAME_GEOMETRY + +set FRAME_COUNT 4 +set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm b/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm new file mode 100644 index 000000000..3ea5479cb --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm @@ -0,0 +1,17 @@ +#!nsh + +# Maintainers: Anton Babushkin , Lorenz Meier + +echo "[init] PX4FMU v1, v2 with or without IO on generic hexacopter X with PWM ESCs" + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # +fi + +set FRAME_TYPE mc +set FRAME_GEOMETRY x +set FRAME_COUNT 6 +set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 deleted file mode 100644 index acd8027fb..000000000 --- a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 +++ /dev/null @@ -1,76 +0,0 @@ -#!nsh - -echo "[init] 666_fmu_q_x550: PX4FMU Quad X550 with or without IO" - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set MC_ATTRATE_P 0.14 - param set MC_ATTRATE_I 0 - param set MC_ATTRATE_D 0.006 - param set MC_ATT_P 5.5 - param set MC_ATT_I 0 - param set MC_ATT_D 0 - param set MC_YAWPOS_D 0 - param set MC_YAWPOS_I 0 - param set MC_YAWPOS_P 0.6 - param set MC_YAWRATE_D 0 - param set MC_YAWRATE_I 0 - param set MC_YAWRATE_P 0.08 - param set RC_SCALE_PITCH 1 - param set RC_SCALE_ROLL 1 - param set RC_SCALE_YAW 3 - - param set SYS_AUTOCONFIG 0 - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - usleep 5000 - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer -# -mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix - -# -# Set PWM output frequency -# -pwm rate -c 1234 -r 400 - -# -# Set disarmed, min and max PWM signals -# -pwm disarmed -c 1234 -p 900 -pwm min -c 1234 -p 1100 -pwm max -c 1234 -p 1900 - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor diff --git a/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm b/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm new file mode 100644 index 000000000..f747618b8 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm @@ -0,0 +1,17 @@ +#!nsh + +# Maintainers: Anton Babushkin , Lorenz Meier + +echo "[init] PX4FMU v1, v2 with or without IO on generic hexacopter + with PWM ESCs" + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # +fi + +set FRAME_TYPE mc +set FRAME_GEOMETRY + +set FRAME_COUNT 4 +set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm b/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm new file mode 100644 index 000000000..f8f459185 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm @@ -0,0 +1,17 @@ +#!nsh + +# Maintainers: Anton Babushkin , Lorenz Meier + +echo "[init] PX4FMU v1, v2 with or without IO on generic octocopter X with PWM ESCs" + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # +fi + +set FRAME_TYPE mc +set FRAME_GEOMETRY x +set FRAME_COUNT 8 +set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm b/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm new file mode 100644 index 000000000..f8bcd13a9 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm @@ -0,0 +1,17 @@ +#!nsh + +# Maintainers: Anton Babushkin , Lorenz Meier + +echo "[init] PX4FMU v1, v2 with or without IO on generic octocopter + with PWM ESCs" + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # +fi + +set FRAME_TYPE mc +set FRAME_GEOMETRY + +set FRAME_COUNT 8 +set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 10b7bc424..153fbb66b 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -17,53 +17,54 @@ # 11000 .. 11999 Hexa Cox # 12000 .. 12999 Octo Cox -if param compare SYS_AUTOSTART 4008 8 +if param compare SYS_AUTOSTART 4001 +then + sh /etc/init.d/4001_quad_x_pwm +fi + +if param compare SYS_AUTOSTART 4008 then #sh /etc/init.d/4008_ardrone fi -if param compare SYS_AUTOSTART 4009 9 +if param compare SYS_AUTOSTART 4009 then #sh /etc/init.d/4009_ardrone_flow fi -if param compare SYS_AUTOSTART 4010 10 +if param compare SYS_AUTOSTART 4010 then sh /etc/init.d/4010_dji_f330 fi -if param compare SYS_AUTOSTART 4011 11 +if param compare SYS_AUTOSTART 4011 then sh /etc/init.d/4011_dji_f450 fi if param compare SYS_AUTOSTART 4012 then - #sh /etc/init.d/666_fmu_q_x550 + sh /etc/init.d/4012_hk_x550 fi -if param compare SYS_AUTOSTART 6012 12 +if param compare SYS_AUTOSTART 6001 then - #set MIXER /etc/mixers/FMU_hex_x.mix - #sh /etc/init.d/rc.hexa + sh /etc/init.d/6001_hexa_x_pwm fi -if param compare SYS_AUTOSTART 7013 13 +if param compare SYS_AUTOSTART 7001 then - #set MIXER /etc/mixers/FMU_hex_+.mix - #sh /etc/init.d/rc.hexa + sh /etc/init.d/7001_hexa_+_pwm fi if param compare SYS_AUTOSTART 8001 then - #set MIXER /etc/mixers/FMU_octo_x.mix - #sh /etc/init.d/rc.octo + sh /etc/init.d/8001_octo_x_pwm fi if param compare SYS_AUTOSTART 9001 then - #set MIXER /etc/mixers/FMU_octo_+.mix - #sh /etc/init.d/rc.octo + sh /etc/init.d/9001_octo_+_pwm fi if param compare SYS_AUTOSTART 12001 diff --git a/ROMFS/px4fmu_common/init.d/rc.hexa b/ROMFS/px4fmu_common/init.d/rc.hexa deleted file mode 100644 index 097db28e4..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.hexa +++ /dev/null @@ -1,94 +0,0 @@ -#!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on Hex" - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - - param set MC_ATTRATE_D 0.004 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.12 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 - param set MC_ATT_P 7.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_P 2.0 - param set MC_YAWRATE_D 0.005 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_P 0.3 - param set NAV_TAKEOFF_ALT 3.0 - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.7 - param set MPC_THR_MIN 0.3 - param set MPC_XY_D 0 - param set MPC_XY_P 0.5 - param set MPC_XY_VEL_D 0 - param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 3 - param set MPC_XY_VEL_P 0.2 - param set MPC_Z_D 0 - param set MPC_Z_P 1 - param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.1 - param set MPC_Z_VEL_MAX 2 - param set MPC_Z_VEL_P 0.20 - - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE list: https://pixhawk.ethz.ch/mavlink/ -# 13 = hexarotor -# -param set MAV_TYPE 13 - -set EXIT_ON_END no - -# -# Start and configure PX4IO interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io -else - # This is not possible on a hexa - tone_alarm error -fi - -# -# Load mixer -# -mixer load /dev/pwm_output $MIXER - -# -# Set PWM output frequency to 400 Hz -# -pwm rate -a -r 400 - -# -# Set disarmed, min and max PWM signals -# -pwm disarmed -c 123456 -p 900 -pwm min -c 123456 -p 1100 -pwm max -c 123456 -p 1900 - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor - -if [ $EXIT_ON_END == yes ] -then - exit -fi diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_interface b/ROMFS/px4fmu_common/init.d/rc.mc_interface index 2a05012a6..9e3d678bf 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_interface +++ b/ROMFS/px4fmu_common/init.d/rc.mc_interface @@ -3,38 +3,34 @@ # Script to set PWM min / max limits and mixer # -# -# Load mixer -# -if [ $FRAME_GEOMETRY == x ] -then - echo "Frame geometry X" - mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix -else - if [ $FRAME_GEOMETRY == w ] - then - echo "Frame geometry W" - mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix - else - echo "Frame geometry +" - mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix - fi -fi - +echo "Rotors count: $FRAME_COUNT" if [ $FRAME_COUNT == 4 ] then + set FRAME_COUNT_STR quad set OUTPUTS 1234 param set MAV_TYPE 2 -else - if [ $FRAME_COUNT == 6 ] - then +fi +if [ $FRAME_COUNT == 6 ] +then + set FRAME_COUNT_STR hex set OUTPUTS 123456 param set MAV_TYPE 13 - else +fi +if [ $FRAME_COUNT == 8 ] +then + set FRAME_COUNT_STR octo set OUTPUTS 12345678 - fi + param set MAV_TYPE 14 fi +# +# Load mixer +# +echo "Frame geometry: ${FRAME_GEOMETRY}" +set MIXER /etc/mixers/FMU_${FRAME_GEOMETRY_STR}_${FRAME_GEOMETRY}.mix +echo "Loading mixer: ${MIXER}" +mixer load /dev/pwm_output ${MIXER} + # # Set PWM output frequency # -- cgit v1.2.3 From b5d56523bc100d7bf95a6dfbac95c1afc89e345e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 10 Jan 2014 13:18:34 +0100 Subject: Init scripts cleanup, WIP --- ROMFS/px4fmu_common/init.d/10015_tbs_discovery | 85 ++---- ROMFS/px4fmu_common/init.d/10016_3dr_iris | 98 +++---- ROMFS/px4fmu_common/init.d/1001_rc_quad.hil | 7 +- ROMFS/px4fmu_common/init.d/2100_mpx_easystar | 58 +--- ROMFS/px4fmu_common/init.d/4001_quad_x_pwm | 17 -- ROMFS/px4fmu_common/init.d/4010_dji_f330 | 10 +- ROMFS/px4fmu_common/init.d/4011_dji_f450 | 12 +- ROMFS/px4fmu_common/init.d/4012_hk_x550 | 8 +- ROMFS/px4fmu_common/init.d/40_io_segway | 51 ---- ROMFS/px4fmu_common/init.d/5001_quad_+_pwm | 17 -- ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm | 17 -- ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm | 17 -- ROMFS/px4fmu_common/init.d/8001_octo_x_pwm | 17 -- ROMFS/px4fmu_common/init.d/800_sdlogger | 53 ---- ROMFS/px4fmu_common/init.d/9001_octo_+_pwm | 17 -- ROMFS/px4fmu_common/init.d/rc.autostart | 154 ++++------- ROMFS/px4fmu_common/init.d/rc.autostart_hil | 34 --- .../init.d/rc.custom_dji_f330_mkblctrl | 113 -------- ROMFS/px4fmu_common/init.d/rc.custom_io_esc | 120 -------- ROMFS/px4fmu_common/init.d/rc.fixedwing | 34 --- ROMFS/px4fmu_common/init.d/rc.fw_apps | 19 ++ ROMFS/px4fmu_common/init.d/rc.fw_interface | 18 ++ ROMFS/px4fmu_common/init.d/rc.mc_apps | 2 +- ROMFS/px4fmu_common/init.d/rc.mc_interface | 70 ++--- ROMFS/px4fmu_common/init.d/rc.octo | 94 ------- ROMFS/px4fmu_common/init.d/rc.standalone | 13 - ROMFS/px4fmu_common/init.d/rc.usb | 33 --- ROMFS/px4fmu_common/init.d/rcS | 306 +++++++++++---------- 28 files changed, 372 insertions(+), 1122 deletions(-) delete mode 100644 ROMFS/px4fmu_common/init.d/4001_quad_x_pwm delete mode 100644 ROMFS/px4fmu_common/init.d/40_io_segway delete mode 100644 ROMFS/px4fmu_common/init.d/5001_quad_+_pwm delete mode 100644 ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm delete mode 100644 ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm delete mode 100644 ROMFS/px4fmu_common/init.d/8001_octo_x_pwm delete mode 100644 ROMFS/px4fmu_common/init.d/800_sdlogger delete mode 100644 ROMFS/px4fmu_common/init.d/9001_octo_+_pwm delete mode 100644 ROMFS/px4fmu_common/init.d/rc.autostart_hil delete mode 100644 ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl delete mode 100644 ROMFS/px4fmu_common/init.d/rc.custom_io_esc delete mode 100644 ROMFS/px4fmu_common/init.d/rc.fixedwing create mode 100644 ROMFS/px4fmu_common/init.d/rc.fw_apps create mode 100644 ROMFS/px4fmu_common/init.d/rc.fw_interface delete mode 100644 ROMFS/px4fmu_common/init.d/rc.octo delete mode 100644 ROMFS/px4fmu_common/init.d/rc.standalone diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery index 81d4b5d57..610d482c1 100644 --- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -1,74 +1,31 @@ #!nsh +# Maintainers: Anton Babushkin + echo "[init] Team Blacksheep Discovery Quad" -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - - param set MC_ATTRATE_D 0.006 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.17 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 + # + # Default parameters for this platform + # param set MC_ATT_P 5.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.15 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_ATTRATE_P 0.17 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.006 param set MC_YAWPOS_P 0.5 - param set MC_YAWRATE_D 0.0 - param set MC_YAWRATE_I 0.0 + param set MC_YAWPOS_I 0.15 + param set MC_YAWPOS_D 0.0 param set MC_YAWRATE_P 0.2 - - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - usleep 5000 - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_D 0.0 fi -# -# Load the mixer for a quad with wide arms -# -mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix - -# -# Set PWM output frequency -# -pwm rate -c 1234 -r 400 - -# -# Set disarmed, min and max PWM signals -# -pwm disarmed -c 1234 -p 900 -pwm min -c 1234 -p 1100 -pwm max -c 1234 -p 1900 - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor +set FRAME_TYPE mc +set FRAME_GEOMETRY quad_w +set PWM_RATE 400 +set PWM_DISARMED 900 +set PWM_MIN 1100 +set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index b0f4eda79..de5028052 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -1,73 +1,43 @@ #!nsh +# Maintainers: Anton Babushkin + echo "[init] 3DR Iris Quad" -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 +# Use generic wide arm quad X PWM as base +sh /etc/init.d/10001_quad_w - param set MC_ATTRATE_D 0.004 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.13 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # param set MC_ATT_P 9.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.15 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_ATTRATE_P 0.13 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 param set MC_YAWPOS_P 0.5 - param set MC_YAWRATE_D 0.0 - param set MC_YAWRATE_I 0.0 + param set MC_YAWPOS_I 0.15 + param set MC_YAWPOS_D 0.0 param set MC_YAWRATE_P 0.2 - - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - usleep 5000 - fmu mode_pwm - param set BAT_V_SCALING 0.0098 - set EXIT_ON_END yes + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_D 0.0 + + param set MPC_TILT_MAX 0.5 + param set MPC_THR_MAX 0.8 + param set MPC_THR_MIN 0.2 + param set MPC_XY_D 0 + param set MPC_XY_P 0.5 + param set MPC_XY_VEL_D 0 + param set MPC_XY_VEL_I 0 + param set MPC_XY_VEL_MAX 3 + param set MPC_XY_VEL_P 0.2 + param set MPC_Z_D 0 + param set MPC_Z_P 1 + param set MPC_Z_VEL_D 0 + param set MPC_Z_VEL_I 0.1 + param set MPC_Z_VEL_MAX 2 + param set MPC_Z_VEL_P 0.20 fi - -# -# Load the mixer for a quad with wide arms -# -mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix - -# -# Set PWM output frequency -# -pwm rate -c 1234 -r 400 - -# -# Set disarmed, min and max PWM signals -# -pwm disarmed -c 1234 -p 900 -pwm min -c 1234 -p 1050 - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil index 8732673f7..7d21176f2 100644 --- a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil @@ -37,6 +37,7 @@ then param set MPC_Z_VEL_P 0.20 fi -set FRAME_TYPE mc -set FRAME_GEOMETRY x -set FRAME_COUNT 4 +set HIL yes + +set VEHICLE_TYPE mc +set FRAME_GEOMETRY quad_x diff --git a/ROMFS/px4fmu_common/init.d/2100_mpx_easystar b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar index 97c2d7c90..9bf01c60c 100644 --- a/ROMFS/px4fmu_common/init.d/2100_mpx_easystar +++ b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar @@ -1,13 +1,12 @@ #!nsh -echo "[init] PX4FMU v1, v2 with or without IO on EasyStar" +echo "[init] EasyStar" -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then - # Set all params here, then disable autoconfig + # + # Default parameters for this platform + # param set FW_P_D 0 param set FW_P_I 0 param set FW_P_IMAX 15 @@ -31,50 +30,7 @@ then param set FW_L1_PERIOD 16 param set RC_SCALE_ROLL 1.0 param set RC_SCALE_PITCH 1.0 - - param set SYS_AUTOCONFIG 0 - param save fi -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - - sh /etc/init.d/rc.io - # Limit to 100 Hz updates and (implicit) 50 Hz PWM - px4io limit 100 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_RET.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_RET.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_RET.mix -else - echo "Using /etc/mixers/FMU_RET.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_RET.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing +set FRAME_TYPE fw +set FRAME_GEOMETRY RET diff --git a/ROMFS/px4fmu_common/init.d/4001_quad_x_pwm b/ROMFS/px4fmu_common/init.d/4001_quad_x_pwm deleted file mode 100644 index 9bee414dc..000000000 --- a/ROMFS/px4fmu_common/init.d/4001_quad_x_pwm +++ /dev/null @@ -1,17 +0,0 @@ -#!nsh - -# Maintainers: Anton Babushkin , Lorenz Meier - -echo "[init] PX4FMU v1, v2 with or without IO on generic quadcopter X with PWM ESCs" - -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # -fi - -set FRAME_TYPE mc -set FRAME_GEOMETRY x -set FRAME_COUNT 4 -set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index 93edb0005..0cfe68b27 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -2,10 +2,7 @@ # Maintainers: Anton Babushkin -echo "[init] PX4FMU v1, v2 with or without IO on DJI F330" - -# Use generic quad X PWM as base -sh /etc/init.d/4001_quad_x_pwm +echo "[init] DJI F330" if [ $DO_AUTOCONFIG == yes ] then @@ -42,6 +39,11 @@ then param set MPC_Z_VEL_P 0.20 fi +set VEHICLE_TYPE mc +set FRAME_GEOMETRY quad_x + +set PWM_RATE 400 + # DJI ESC range set PWM_DISARMED 900 set PWM_MIN 1200 diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index cc0ddccc8..4a0953a2e 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -2,10 +2,7 @@ # Maintainers: Lorenz Meier -echo "[init] PX4FMU v1, v2 with or without IO on DJI F450" - -# Use generic quad X PWM as base -sh /etc/init.d/4001_quad_x_pwm +echo "[init] DJI F450" if [ $DO_AUTOCONFIG == yes ] then @@ -27,7 +24,12 @@ then # TODO add default MPC parameters fi - + +set VEHICLE_TYPE mc +set FRAME_GEOMETRY quad_x + +set PWM_RATE 400 + # DJI ESC range set PWM_DISARMED 900 set PWM_MIN 1200 diff --git a/ROMFS/px4fmu_common/init.d/4012_hk_x550 b/ROMFS/px4fmu_common/init.d/4012_hk_x550 index a749b7699..396349d9c 100644 --- a/ROMFS/px4fmu_common/init.d/4012_hk_x550 +++ b/ROMFS/px4fmu_common/init.d/4012_hk_x550 @@ -4,9 +4,6 @@ echo "[init] PX4FMU v1, v2 with or without IO on HobbyKing X550" -# Use generic quad X PWM as base -sh /etc/init.d/4001_quad_x_pwm - if [ $DO_AUTOCONFIG == yes ] then # @@ -25,3 +22,8 @@ then param set MC_YAWRATE_I 0 param set MC_YAWRATE_D 0 fi + +set VEHICLE_TYPE mc +set FRAME_GEOMETRY quad_x + +set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/40_io_segway b/ROMFS/px4fmu_common/init.d/40_io_segway deleted file mode 100644 index ad455b440..000000000 --- a/ROMFS/px4fmu_common/init.d/40_io_segway +++ /dev/null @@ -1,51 +0,0 @@ -#!nsh - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - # TODO - - param set SYS_AUTOCONFIG 0 - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 10 = ground rover -# -param set MAV_TYPE 10 - -# -# Start MAVLink (depends on orb) -# -mavlink start -d /dev/ttyS1 -b 57600 -usleep 5000 - -# -# Start and configure PX4IO interface -# -sh /etc/init.d/rc.io - -# -# Start the sensors (depends on orb, px4io) -# -sh /etc/init.d/rc.sensors - -# -# Start GPS interface (depends on orb) -# -gps start - -# -# Start the attitude estimator (depends on orb) -# -attitude_estimator_ekf start - -# -# Load mixer and start controllers (depends on px4io) -# -roboclaw start /dev/ttyS2 128 1200 -segway start diff --git a/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm b/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm deleted file mode 100644 index 7d0748668..000000000 --- a/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm +++ /dev/null @@ -1,17 +0,0 @@ -#!nsh - -# Maintainers: Anton Babushkin , Lorenz Meier - -echo "[init] PX4FMU v1, v2 with or without IO on generic quadcopter + with PWM ESCs" - -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # -fi - -set FRAME_TYPE mc -set FRAME_GEOMETRY + -set FRAME_COUNT 4 -set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm b/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm deleted file mode 100644 index 3ea5479cb..000000000 --- a/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm +++ /dev/null @@ -1,17 +0,0 @@ -#!nsh - -# Maintainers: Anton Babushkin , Lorenz Meier - -echo "[init] PX4FMU v1, v2 with or without IO on generic hexacopter X with PWM ESCs" - -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # -fi - -set FRAME_TYPE mc -set FRAME_GEOMETRY x -set FRAME_COUNT 6 -set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm b/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm deleted file mode 100644 index f747618b8..000000000 --- a/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm +++ /dev/null @@ -1,17 +0,0 @@ -#!nsh - -# Maintainers: Anton Babushkin , Lorenz Meier - -echo "[init] PX4FMU v1, v2 with or without IO on generic hexacopter + with PWM ESCs" - -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # -fi - -set FRAME_TYPE mc -set FRAME_GEOMETRY + -set FRAME_COUNT 4 -set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm b/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm deleted file mode 100644 index f8f459185..000000000 --- a/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm +++ /dev/null @@ -1,17 +0,0 @@ -#!nsh - -# Maintainers: Anton Babushkin , Lorenz Meier - -echo "[init] PX4FMU v1, v2 with or without IO on generic octocopter X with PWM ESCs" - -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # -fi - -set FRAME_TYPE mc -set FRAME_GEOMETRY x -set FRAME_COUNT 8 -set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/800_sdlogger b/ROMFS/px4fmu_common/init.d/800_sdlogger deleted file mode 100644 index 2d2c3737b..000000000 --- a/ROMFS/px4fmu_common/init.d/800_sdlogger +++ /dev/null @@ -1,53 +0,0 @@ -#!nsh - -echo "[init] PX4FMU v1, v2 init to log only - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - - param save -fi - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io - # Set PWM values for DJI ESCs -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - usleep 5000 - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -sh /etc/init.d/rc.sensors - -gps start - -attitude_estimator_ekf start - -position_estimator_inav start - -if [ -d /fs/microsd ] -then - if hw_ver compare PX4FMU_V1 - then - echo "Start sdlog2 at 50Hz" - sdlog2 start -r 50 -e -b 16 - else - echo "Start sdlog2 at 200Hz" - sdlog2 start -r 200 -e -b 16 - fi -fi diff --git a/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm b/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm deleted file mode 100644 index f8bcd13a9..000000000 --- a/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm +++ /dev/null @@ -1,17 +0,0 @@ -#!nsh - -# Maintainers: Anton Babushkin , Lorenz Meier - -echo "[init] PX4FMU v1, v2 with or without IO on generic octocopter + with PWM ESCs" - -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # -fi - -set FRAME_TYPE mc -set FRAME_GEOMETRY + -set FRAME_COUNT 8 -set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 153fbb66b..9da0135b3 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -17,114 +17,62 @@ # 11000 .. 11999 Hexa Cox # 12000 .. 12999 Octo Cox -if param compare SYS_AUTOSTART 4001 -then - sh /etc/init.d/4001_quad_x_pwm -fi - -if param compare SYS_AUTOSTART 4008 -then - #sh /etc/init.d/4008_ardrone -fi - -if param compare SYS_AUTOSTART 4009 -then - #sh /etc/init.d/4009_ardrone_flow -fi - -if param compare SYS_AUTOSTART 4010 -then - sh /etc/init.d/4010_dji_f330 -fi - -if param compare SYS_AUTOSTART 4011 -then - sh /etc/init.d/4011_dji_f450 -fi - -if param compare SYS_AUTOSTART 4012 -then - sh /etc/init.d/4012_hk_x550 -fi - -if param compare SYS_AUTOSTART 6001 -then - sh /etc/init.d/6001_hexa_x_pwm -fi - -if param compare SYS_AUTOSTART 7001 -then - sh /etc/init.d/7001_hexa_+_pwm -fi - -if param compare SYS_AUTOSTART 8001 -then - sh /etc/init.d/8001_octo_x_pwm -fi +# +# Simulation setups +# -if param compare SYS_AUTOSTART 9001 +if param compare SYS_AUTOSTART 1000 then - sh /etc/init.d/9001_octo_+_pwm + #sh /etc/init.d/1000_rc_fw_easystar.hil fi -if param compare SYS_AUTOSTART 12001 +if param compare SYS_AUTOSTART 1001 then - #set MIXER /etc/mixers/FMU_octo_cox.mix - #sh /etc/init.d/rc.octo + sh /etc/init.d/1001_rc_quad.hil fi -if param compare SYS_AUTOSTART 10015 15 +if param compare SYS_AUTOSTART 1002 then - #sh /etc/init.d/10015_tbs_discovery + #sh /etc/init.d/1002_rc_fw_state.hil fi -if param compare SYS_AUTOSTART 10016 16 +if param compare SYS_AUTOSTART 1003 then - #sh /etc/init.d/10016_3dr_iris + #sh /etc/init.d/1003_rc_quad_+.hil fi -if param compare SYS_AUTOSTART 4017 17 +if param compare SYS_AUTOSTART 1004 then - #set MKBLCTRL_MODE no - #set MKBLCTRL_FRAME x - #sh /etc/init.d/rc.custom_dji_f330_mkblctrl + #sh /etc/init.d/1004_rc_fw_Rascal110.hil fi -if param compare SYS_AUTOSTART 5018 18 -then - #set MKBLCTRL_MODE no - #set MKBLCTRL_FRAME + - #sh /etc/init.d/rc.custom_dji_f330_mkblctrl -fi +# +# Standard plane +# -if param compare SYS_AUTOSTART 4019 19 +if param compare SYS_AUTOSTART 2100 100 then - #set MKBLCTRL_MODE yes - #set MKBLCTRL_FRAME x - #sh /etc/init.d/rc.custom_dji_f330_mkblctrl + #sh /etc/init.d/2100_mpx_easystar + #set MODE custom fi -if param compare SYS_AUTOSTART 5020 20 +if param compare SYS_AUTOSTART 2101 101 then - #set MKBLCTRL_MODE yes - #set MKBLCTRL_FRAME + - #sh /etc/init.d/rc.custom_dji_f330_mkblctrl + #sh /etc/init.d/2101_hk_bixler + #set MODE custom fi -if param compare SYS_AUTOSTART 4021 21 +if param compare SYS_AUTOSTART 2102 102 then - #set FRAME_GEOMETRY x - #set ESC_MAKER afro - #sh /etc/init.d/rc.custom_io_esc + #sh /etc/init.d/2102_3dr_skywalker + #set MODE custom fi -if param compare SYS_AUTOSTART 10022 22 -then - #set FRAME_GEOMETRY w - #sh /etc/init.d/rc.custom_io_esc -fi +# +# Flying wing +# -if param compare SYS_AUTOSTART 3030 30 +if param compare SYS_AUTOSTART 3030 then #sh /etc/init.d/3030_io_camflyer fi @@ -147,35 +95,47 @@ fi if param compare SYS_AUTOSTART 3034 34 then #sh /etc/init.d/3034_io_fx79 - #set MODE custom fi -if param compare SYS_AUTOSTART 40 +# +# Quad X +# + +if param compare SYS_AUTOSTART 4008 then - #sh /etc/init.d/40_io_segway - #set MODE custom + #sh /etc/init.d/4008_ardrone fi -if param compare SYS_AUTOSTART 2100 100 +if param compare SYS_AUTOSTART 4009 then - #sh /etc/init.d/2100_mpx_easystar - #set MODE custom + #sh /etc/init.d/4009_ardrone_flow fi -if param compare SYS_AUTOSTART 2101 101 +if param compare SYS_AUTOSTART 4010 then - #sh /etc/init.d/2101_hk_bixler - #set MODE custom + sh /etc/init.d/4010_dji_f330 fi -if param compare SYS_AUTOSTART 2102 102 +if param compare SYS_AUTOSTART 4011 then - #sh /etc/init.d/2102_3dr_skywalker - #set MODE custom + sh /etc/init.d/4011_dji_f450 +fi + +if param compare SYS_AUTOSTART 4012 +then + sh /etc/init.d/4012_hk_x550 fi -if param compare SYS_AUTOSTART 800 +# +# Wide arm / H frame +# + +if param compare SYS_AUTOSTART 10015 then - #sh /etc/init.d/800_sdlogger - #set MODE custom + sh /etc/init.d/10015_tbs_discovery +fi + +if param compare SYS_AUTOSTART 10016 +then + sh /etc/init.d/10016_3dr_iris fi diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart_hil b/ROMFS/px4fmu_common/init.d/rc.autostart_hil deleted file mode 100644 index d5fc5eb08..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.autostart_hil +++ /dev/null @@ -1,34 +0,0 @@ -# -# Check if auto-setup from one of the standard scripts for HIL is wanted -# - -if param compare SYS_AUTOSTART 1000 -then - #sh /etc/init.d/1000_rc_fw_easystar.hil - set MODE hil -fi - -if param compare SYS_AUTOSTART 1001 -then - sh /etc/init.d/1001_rc_quad.hil - set MODE hil -fi - -if param compare SYS_AUTOSTART 1002 -then - #sh /etc/init.d/1002_rc_fw_state.hil - set MODE hil -fi - -if param compare SYS_AUTOSTART 1003 -then - #sh /etc/init.d/1003_rc_quad_+.hil - set MODE hil -fi - -if param compare SYS_AUTOSTART 1004 -then - #sh /etc/init.d/1004_rc_fw_Rascal110.hil - set MODE hil -fi - diff --git a/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl b/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl deleted file mode 100644 index 40b2ee68b..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl +++ /dev/null @@ -1,113 +0,0 @@ -#!nsh - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - - param set MC_ATTRATE_D 0.002 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.09 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 - param set MC_ATT_P 6.8 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_P 2.0 - param set MC_YAWRATE_D 0.005 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_P 0.3 - param set NAV_TAKEOFF_ALT 3.0 - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.7 - param set MPC_THR_MIN 0.3 - param set MPC_XY_D 0 - param set MPC_XY_P 0.5 - param set MPC_XY_VEL_D 0 - param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 3 - param set MPC_XY_VEL_P 0.2 - param set MPC_Z_D 0 - param set MPC_Z_P 1 - param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.1 - param set MPC_Z_VEL_MAX 2 - param set MPC_Z_VEL_P 0.20 - - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 - -set EXIT_ON_END no - -# -# Start the Mikrokopter ESC driver -# -if [ $MKBLCTRL_MODE == yes ] -then - if [ $MKBLCTRL_FRAME == x ] - then - echo "[init] PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame Mikrokopter-Addressing" - mkblctrl -mkmode x - else - echo "[init] PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame Mikrokopter-Addressing" - mkblctrl -mkmode + - fi -else - if [ $MKBLCTRL_FRAME == x ] - then - echo "[init] PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame" - else - echo "[init] PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame" - fi - mkblctrl -fi - -usleep 10000 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - usleep 5000 - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer -# -if [ $MKBLCTRL_FRAME == x ] -then - mixer load /dev/mkblctrl /etc/mixers/FMU_quad_x.mix -else - mixer load /dev/mkblctrl /etc/mixers/FMU_quad_+.mix -fi - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor - -if [ $EXIT_ON_END == yes ] -then - exit -fi diff --git a/ROMFS/px4fmu_common/init.d/rc.custom_io_esc b/ROMFS/px4fmu_common/init.d/rc.custom_io_esc deleted file mode 100644 index 045e41e52..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.custom_io_esc +++ /dev/null @@ -1,120 +0,0 @@ -#!nsh - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - - param set MC_ATTRATE_D 0.002 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.09 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 - param set MC_ATT_P 6.8 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_P 2.0 - param set MC_YAWRATE_D 0.005 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_P 0.3 - param set NAV_TAKEOFF_ALT 3.0 - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.7 - param set MPC_THR_MIN 0.3 - param set MPC_XY_D 0 - param set MPC_XY_P 0.5 - param set MPC_XY_VEL_D 0 - param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 3 - param set MPC_XY_VEL_P 0.2 - param set MPC_Z_D 0 - param set MPC_Z_P 1 - param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.1 - param set MPC_Z_VEL_MAX 2 - param set MPC_Z_VEL_P 0.20 - - param save -fi - -echo "RC script for PX4FMU + PX4IO + PPM-ESCs running" - -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 - -set EXIT_ON_END no - -usleep 10000 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start -d /dev/ttyS1 -b 57600 - usleep 5000 - - sh /etc/init.d/rc.io -else - fmu mode_pwm - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS1 -b 57600 - usleep 5000 - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -if [ $ESC_MAKER = afro ] -then - # Set PWM values for Afro ESCs - pwm disarmed -c 1234 -p 1050 - pwm min -c 1234 -p 1080 - pwm max -c 1234 -p 1860 -else - # Set PWM values for typical ESCs - pwm disarmed -c 1234 -p 900 - pwm min -c 1234 -p 980 - pwm max -c 1234 -p 1800 -fi - -# -# Load mixer -# -if [ $FRAME_GEOMETRY == x ] -then - echo "Frame geometry X" - mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix -else - if [ $FRAME_GEOMETRY == w ] - then - echo "Frame geometry W" - mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix - else - echo "Frame geometry +" - mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix - fi -fi - -# -# Set PWM output frequency -# -pwm rate -r 400 -c 1234 - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor - -if [ $EXIT_ON_END == yes ] -then - exit -fi - -echo "Script end" diff --git a/ROMFS/px4fmu_common/init.d/rc.fixedwing b/ROMFS/px4fmu_common/init.d/rc.fixedwing deleted file mode 100644 index f02851006..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.fixedwing +++ /dev/null @@ -1,34 +0,0 @@ -#!nsh -# -# Standard everything needed for fixedwing except mixer, actuator output and mavlink -# - -# -# Start the sensors and test them. -# -sh /etc/init.d/rc.sensors - -# -# Start logging (depends on sensors) -# -sh /etc/init.d/rc.logging - -# -# Start GPS interface (depends on orb) -# -gps start - -# -# Start the attitude and position estimator -# -att_pos_estimator_ekf start - -# -# Start attitude controller -# -fw_att_control start - -# -# Start the position controller -# -fw_pos_control_l1 start diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps new file mode 100644 index 000000000..d354fb06f --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -0,0 +1,19 @@ +#!nsh +# +# Standard apps for fixed wing +# + +# +# Start the attitude and position estimator +# +att_pos_estimator_ekf start + +# +# Start attitude controller +# +fw_att_control start + +# +# Start the position controller +# +fw_pos_control_l1 start diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_interface b/ROMFS/px4fmu_common/init.d/rc.fw_interface new file mode 100644 index 000000000..f9517f422 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.fw_interface @@ -0,0 +1,18 @@ +#!nsh +# +# Script to load fixedwing mixer and set rate/disarmed/min/max values for PWM output +# + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing +# +param set MAV_TYPE 1 + +# +# Load mixer +# +echo "Frame geometry: ${FRAME_GEOMETRY}" +set MIXER /etc/mixers/FMU_${FRAME_GEOMETRY}.mix +echo "Loading mixer: ${MIXER}" +mixer load /dev/pwm_output ${MIXER} diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index 16a7a33c6..8b51d57e5 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -1,6 +1,6 @@ #!nsh # -# Standard everything needed for multirotors except mixer, actuator output and mavlink +# Standard apps for multirotors # # diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_interface b/ROMFS/px4fmu_common/init.d/rc.mc_interface index 9e3d678bf..003aee81b 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_interface +++ b/ROMFS/px4fmu_common/init.d/rc.mc_interface @@ -1,24 +1,25 @@ #!nsh # -# Script to set PWM min / max limits and mixer +# Script to load multicopter mixer and set rate/disarmed/min/max values for PWM output # -echo "Rotors count: $FRAME_COUNT" -if [ $FRAME_COUNT == 4 ] +if [ $FRAME_GEOMETRY == quad_x -o $FRAME_GEOMETRY == quad_+ ] then - set FRAME_COUNT_STR quad set OUTPUTS 1234 param set MAV_TYPE 2 fi -if [ $FRAME_COUNT == 6 ] +if [ $FRAME_GEOMETRY == quad_w -o $FRAME_GEOMETRY == quad_v ] +then + set OUTPUTS 1234 + param set MAV_TYPE 2 +fi +if [ $FRAME_GEOMETRY == hex_x -o $FRAME_GEOMETRY == hex_+ ] then - set FRAME_COUNT_STR hex set OUTPUTS 123456 param set MAV_TYPE 13 fi -if [ $FRAME_COUNT == 8 ] +if [ $FRAME_GEOMETRY == octo_x -o $FRAME_GEOMETRY == octo_+ ] then - set FRAME_COUNT_STR octo set OUTPUTS 12345678 param set MAV_TYPE 14 fi @@ -26,31 +27,34 @@ fi # # Load mixer # -echo "Frame geometry: ${FRAME_GEOMETRY}" -set MIXER /etc/mixers/FMU_${FRAME_GEOMETRY_STR}_${FRAME_GEOMETRY}.mix -echo "Loading mixer: ${MIXER}" -mixer load /dev/pwm_output ${MIXER} +echo "Frame geometry: $FRAME_GEOMETRY" +set MIXER /etc/mixers/FMU_${FRAME_GEOMETRY}.mix +echo "Loading mixer: $MIXER" +mixer load /dev/pwm_output $MIXER -# -# Set PWM output frequency -# -if [ $PWM_RATE != none ] -then - pwm rate -c $OUTPUTS -r $PWM_RATE -fi - -# -# Set disarmed, min and max PWM values -# -if [ $PWM_DISARMED != none ] -then - pwm disarmed -c $OUTPUTS -p $PWM_DISARMED -fi -if [ $PWM_MIN != none ] -then - pwm min -c $OUTPUTS -p $PWM_MIN -fi -if [ $PWM_MAX != none ] +if [ $OUTPUT_MODE == fmu_pwm -o $OUTPUT_MODE == io_pwm ] then - pwm max -c $OUTPUTS -p $PWM_MAX + # + # Set PWM output frequency + # + if [ $PWM_RATE != none ] + then + pwm rate -c $OUTPUTS -r $PWM_RATE + fi + + # + # Set disarmed, min and max PWM values + # + if [ $PWM_DISARMED != none ] + then + pwm disarmed -c $OUTPUTS -p $PWM_DISARMED + fi + if [ $PWM_MIN != none ] + then + pwm min -c $OUTPUTS -p $PWM_MIN + fi + if [ $PWM_MAX != none ] + then + pwm max -c $OUTPUTS -p $PWM_MAX + fi fi diff --git a/ROMFS/px4fmu_common/init.d/rc.octo b/ROMFS/px4fmu_common/init.d/rc.octo deleted file mode 100644 index ecb12e96e..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.octo +++ /dev/null @@ -1,94 +0,0 @@ -#!nsh - -echo "[init] Octorotor startup" - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - - param set MC_ATTRATE_D 0.004 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.12 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 - param set MC_ATT_P 7.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_P 2.0 - param set MC_YAWRATE_D 0.005 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_P 0.3 - param set NAV_TAKEOFF_ALT 3.0 - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.7 - param set MPC_THR_MIN 0.3 - param set MPC_XY_D 0 - param set MPC_XY_P 0.5 - param set MPC_XY_VEL_D 0 - param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 3 - param set MPC_XY_VEL_P 0.2 - param set MPC_Z_D 0 - param set MPC_Z_P 1 - param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.1 - param set MPC_Z_VEL_MAX 2 - param set MPC_Z_VEL_P 0.20 - - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE list: https://pixhawk.ethz.ch/mavlink/ -# 14 = octorotor -# -param set MAV_TYPE 14 - -set EXIT_ON_END no - -# -# Start and configure PX4IO interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io -else - # This is not possible on an octo - tone_alarm error -fi - -# -# Load mixer -# -mixer load /dev/pwm_output $MIXER - -# -# Set PWM output frequency to 400 Hz -# -pwm rate -a -r 400 - -# -# Set disarmed, min and max PWM signals -# -pwm disarmed -c 12345678 -p 900 -pwm min -c 12345678 -p 1100 -pwm max -c 12345678 -p 1900 - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor - -if [ $EXIT_ON_END == yes ] -then - exit -fi diff --git a/ROMFS/px4fmu_common/init.d/rc.standalone b/ROMFS/px4fmu_common/init.d/rc.standalone deleted file mode 100644 index 67e95215b..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.standalone +++ /dev/null @@ -1,13 +0,0 @@ -#!nsh -# -# Flight startup script for PX4FMU standalone configuration. -# - -echo "[init] doing standalone PX4FMU startup..." - -# -# Start the ORB -# -uorb start - -echo "[init] startup done" diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index ccf2cd47e..0cd8a0e04 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -36,39 +36,6 @@ then echo "Commander started" fi -# Start px4io if present -if px4io start -then - echo "PX4IO driver started" -else - if fmu mode_serial - then - echo "FMU driver started" - fi -fi - -# Start sensors -sh /etc/init.d/rc.sensors - -# Start one of the estimators -if attitude_estimator_ekf status -then - echo "multicopter att filter running" -else - if att_pos_estimator_ekf status - then - echo "fixedwing att filter running" - else - attitude_estimator_ekf start - fi -fi - -# Start GPS -if gps start -then - echo "GPS started" -fi - echo "MAVLink started, exiting shell.." # Exit shell to make it available to MAVLink diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 7d38736de..09d66cf41 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -52,7 +52,7 @@ then echo "[init] USB interface connected" fi - echo "Running rc.APM" + echo "[init] Running rc.APM" # if APM startup is successful then nsh will exit sh /etc/init.d/rc.APM fi @@ -85,9 +85,9 @@ then then if param load /fs/microsd/params then - echo "Parameters loaded" + echo "[init] Parameters loaded" else - echo "Parameter file corrupt - ignoring" + echo "[init] Parameter file corrupt - ignoring" fi fi #fi @@ -97,7 +97,7 @@ then # if rgbled start then - echo "Using external RGB Led" + echo "[init] Using external RGB Led" else if blinkm start then @@ -105,75 +105,10 @@ then fi fi - set USE_IO no - set FRAME_TYPE none - set PWM_RATE none - set PWM_DISARMED none - set PWM_MIN none - set PWM_MAX none - - if param compare SYS_AUTOCONFIG 1 - then - set DO_AUTOCONFIG yes - else - set DO_AUTOCONFIG no - fi - - # - # Start the Commander (needs to be this early for in-air-restarts) - # - commander start - - # - # Set parameters and env variables for selected AUTOSTART (HIL setups) - # - sh /etc/init.d/rc.autostart_hil + # Use FMU PWM output by default + set OUTPUT_MODE fmu_pwm + set IO_PRESENT no - if [ $MODE == hil ] - then - # - # Do common HIL setup depending on env variables - # - # Allow USB some time to come up - sleep 1 - - # Start MAVLink on USB port - mavlink start -b 230400 -d /dev/ttyACM0 - usleep 5000 - - # Create a fake HIL /dev/pwm_output interface - hil mode_pwm - - # Sensors - echo "Start sensors" - sh /etc/init.d/rc.sensors - - # - # Fixed wing setup - # - if [ $FRAME_TYPE == fw ] - then - echo "Setup FIXED WING" - fi - - # - # Multicopters setup - # - if [ $FRAME_TYPE == mc ] - then - echo "Setup MULTICOPTER" - - # Load mixer and configure outputs - sh /etc/init.d/rc.mc_interface - - # Start common multicopter apps - sh /etc/init.d/rc.mc_apps - fi - else - # Try to get an USB console if not in HIL mode - nshterm /dev/ttyACM0 & - fi - # # Upgrade PX4IO firmware # @@ -184,19 +119,17 @@ then set io_file /etc/extras/px4io-v1_default.bin fi - if px4io start - then - echo "PX4IO OK" - echo "PX4IO OK" >> $logfile - fi - if px4io checkcrc $io_file then - echo "PX4IO CRC OK" + echo "[init] PX4IO CRC OK" echo "PX4IO CRC OK" >> $logfile - set USE_IO yes + + set IO_PRESENT yes + + # If PX4IO present, use it as primary PWM output by default + set OUTPUT_MODE io_pwm else - echo "PX4IO CRC failure" + echo "[init] PX4IO CRC failure" echo "PX4IO CRC failure" >> $logfile tone_alarm MBABGP if px4io forceupdate 14662 $io_file @@ -204,12 +137,16 @@ then usleep 500000 if px4io start then - echo "PX4IO restart OK" + echo "[init] PX4IO restart OK" echo "PX4IO restart OK" >> $logfile tone_alarm MSPAA - set USE_IO yes + + set IO_PRESENT yes + + # If PX4IO present, use it as primary PWM output by default + set OUTPUT_MODE io_pwm else - echo "PX4IO restart failed" + echo "[init] PX4IO restart failed" echo "PX4IO restart failed" >> $logfile if hw_ver compare PX4FMU_V2 then @@ -219,18 +156,54 @@ then fi fi else - echo "PX4IO update failed" + echo "[init] PX4IO update failed" echo "PX4IO update failed" >> $logfile - tone_alarm MNGGG + if hw_ver compare PX4FMU_V2 + then + tone_alarm MNGGG + fi fi fi - + + set HIL no + set FRAME_TYPE none + set PWM_RATE none + set PWM_DISARMED none + set PWM_MIN none + set PWM_MAX none + set EXIT_ON_END no + if param compare SYS_AUTOCONFIG 1 + then + set DO_AUTOCONFIG yes + else + set DO_AUTOCONFIG no + fi + + # + # Start the Commander (needs to be this early for in-air-restarts) + # + commander start + # # Set parameters and env variables for selected AUTOSTART # sh /etc/init.d/rc.autostart + + # Custom configuration + if [ -f /fs/microsd/etc/rc.conf ] + then + sh /fs/microsd/etc/rc.conf + fi + + if [ $HIL == yes ] + then + set OUTPUT_MODE hil + else + # Try to get an USB console if not in HIL mode + nshterm /dev/ttyACM0 & + fi # # If autoconfig parameter was set, reset it and save parameters @@ -240,66 +213,132 @@ then param set SYS_AUTOCONFIG 0 param save fi - + if [ $MODE == autostart ] then # - # Do common setup depending on env variables + # Start primary output # - if [ $USE_IO == yes ] + if [ $OUTPUT_MODE == io_pwm ] then - echo "Use IO" - - # Start MAVLink on default port: ttyS1 - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io - else - echo "Don't use IO" - - # Start MAVLink on ttyS0 + echo "[init] Use PX4IO PWM as primary output" + if px4io start + then + echo "[init] PX4IO OK" + echo "PX4IO OK" >> $logfile + sh /etc/init.d/rc.io + else + echo "[init] PX4IO start error" + echo "PX4IO start error" >> $logfile + tone_alarm MNGGG + fi + fi + if [ $OUTPUT_MODE == fmu_pwm ] + then + echo "[init] Use PX4FMU PWM as primary output" + fmu mode_pwm + fi + if [ $OUTPUT_MODE == mkblctrl ] + then + echo "[init] Use MKBLCTRL as primary output" + mkblctrl + fi + if [ $OUTPUT_MODE == hil ] + then + echo "[init] Use HIL as primary output" + hil mode_pwm + fi + + # + # Start PX4IO as secondary output if needed + # + if [ $IO_PRESENT == yes -a $OUTPUT_MODE != io_pwm ] + then + echo "[init] Use PX4IO PWM as secondary output" + if px4io start + then + echo "[init] PX4IO OK" + echo "PX4IO OK" >> $logfile + sh /etc/init.d/rc.io + else + echo "[init] PX4IO start error" + echo "PX4IO start error" >> $logfile + tone_alarm MNGGG + fi + fi + + # + # MAVLink + # + if [ $OUTPUT_MODE == fmu_pwm -a hw_ver compare PX4FMU_V1 ] + then + # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as PWM output mavlink start -d /dev/ttyS0 usleep 5000 - # Configure FMU for PWM outputs - fmu mode_pwm - # Exit from nsh to free port for mavlink set EXIT_ON_END yes + else + # Start MAVLink on default port: ttyS1 + mavlink start + usleep 5000 fi - # Sensors - echo "Start sensors" + # + # Sensors, Logging, GPS + # + echo "[init] Start sensors" sh /etc/init.d/rc.sensors - - # Logging - sh /etc/init.d/rc.logging - - # GPS interface - gps start + + if [ $HIL == no ] + then + echo "[init] Start logging" + sh /etc/init.d/rc.logging + fi + + if [ $HIL == no ] + then + echo "[init] Start GPS" + gps start + fi # # Fixed wing setup # - if [ $FRAME_TYPE == fw ] + if [ $VEHICLE_TYPE == fw ] then - echo "Setup FIXED WING" + echo "[init] Vehicle type: FIXED WING" + + # Load mixer and configure outputs + sh /etc/init.d/rc.fw_interface + + # Start standard fixedwing apps + sh /etc/init.d/rc.mc_apps fi # # Multicopters setup # - if [ $FRAME_TYPE == mc ] + if [ $VEHICLE_TYPE == mc ] then - echo "Setup MULTICOPTER" + echo "[init] Vehicle type: MULTICOPTER" # Load mixer and configure outputs sh /etc/init.d/rc.mc_interface - # Start common multicopter apps + # Start standard multicopter apps sh /etc/init.d/rc.mc_apps fi + + # + # Generic setup (autostart ID not found) + # + if [ $VEHICLE_TYPE == none ] + then + echo "[init] Vehicle type: GENERIC" + attitude_estimator_ekf start + position_estimator_inav start + fi fi # Start any custom extensions @@ -307,39 +346,6 @@ then then sh /fs/microsd/etc/rc.local fi - - # If none of the autostart scripts triggered, get a minimal setup - if [ $MODE == autostart ] - then - # Telemetry port is on both FMU boards ttyS1 - # but the AR.Drone motors can be get 'flashed' - # if starting MAVLink on them - so do not - # start it as default (default link: USB) - - # Start commander - commander start - - # Start px4io if present - if px4io detect - then - px4io start - else - if fmu mode_serial - then - echo "FMU driver (no PWM) started" - fi - fi - - # Start sensors - sh /etc/init.d/rc.sensors - - # Start one of the estimators - attitude_estimator_ekf start - - # Start GPS - gps start - - fi if [ $EXIT_ON_END == yes ] then -- cgit v1.2.3 From 6e609845569722367b5d38bc508edb69dfa8d47f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 10 Jan 2014 22:04:56 +0100 Subject: rcS and autostart scripts cleanup, WIP --- ROMFS/px4fmu_common/init.d/10015_tbs_discovery | 9 +- ROMFS/px4fmu_common/init.d/10016_3dr_iris | 15 +- ROMFS/px4fmu_common/init.d/1001_rc_quad.hil | 43 ------ ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil | 46 ++++++ ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil | 133 +++++------------ ROMFS/px4fmu_common/init.d/4010_dji_f330 | 7 +- ROMFS/px4fmu_common/init.d/4011_dji_f450 | 7 +- ROMFS/px4fmu_common/init.d/4012_hk_x550 | 9 +- ROMFS/px4fmu_common/init.d/rc.autostart | 4 +- ROMFS/px4fmu_common/init.d/rcS | 199 ++++++++++++++----------- 10 files changed, 223 insertions(+), 249 deletions(-) delete mode 100644 ROMFS/px4fmu_common/init.d/1001_rc_quad.hil create mode 100644 ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery index 610d482c1..24f1099d3 100644 --- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -1,8 +1,9 @@ #!nsh - -# Maintainers: Anton Babushkin - -echo "[init] Team Blacksheep Discovery Quad" +# +# Team Blacksheep Discovery Quadcopter +# +# Maintainers: Simon Wilks +# if [ $DO_AUTOCONFIG == yes ] then diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index de5028052..b8fc5e606 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -1,11 +1,9 @@ #!nsh - +# +# 3DR Iris Quadcopter +# # Maintainers: Anton Babushkin - -echo "[init] 3DR Iris Quad" - -# Use generic wide arm quad X PWM as base -sh /etc/init.d/10001_quad_w +# if [ $DO_AUTOCONFIG == yes ] then @@ -41,3 +39,8 @@ then param set MPC_Z_VEL_MAX 2 param set MPC_Z_VEL_P 0.20 fi + +set VEHICLE_TYPE mc +set FRAME_GEOMETRY quad_w + +set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil deleted file mode 100644 index 7d21176f2..000000000 --- a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil +++ /dev/null @@ -1,43 +0,0 @@ -#!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on HIL Quad" - -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # - param set MC_ATTRATE_P 0.12 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_D 0.004 - param set MC_ATT_P 7.0 - param set MC_ATT_I 0.0 - param set MC_ATT_D 0.0 - param set MC_YAWPOS_P 2.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWRATE_P 0.3 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_D 0.005 - - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.8 - param set MPC_THR_MIN 0.2 - param set MPC_XY_D 0 - param set MPC_XY_P 0.5 - param set MPC_XY_VEL_D 0 - param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 3 - param set MPC_XY_VEL_P 0.2 - param set MPC_Z_D 0 - param set MPC_Z_P 1 - param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.1 - param set MPC_Z_VEL_MAX 2 - param set MPC_Z_VEL_P 0.20 -fi - -set HIL yes - -set VEHICLE_TYPE mc -set FRAME_GEOMETRY quad_x diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil new file mode 100644 index 000000000..c5b92d7d4 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil @@ -0,0 +1,46 @@ +#!nsh +# +# HIL Quadcopter X +# +# Maintainers: Anton Babushkin +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_YAWPOS_P 2.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + param set MPC_TILT_MAX 0.5 + param set MPC_THR_MAX 0.8 + param set MPC_THR_MIN 0.2 + param set MPC_XY_D 0 + param set MPC_XY_P 0.5 + param set MPC_XY_VEL_D 0 + param set MPC_XY_VEL_I 0 + param set MPC_XY_VEL_MAX 3 + param set MPC_XY_VEL_P 0.2 + param set MPC_Z_D 0 + param set MPC_Z_P 1 + param set MPC_Z_VEL_D 0 + param set MPC_Z_VEL_I 0.1 + param set MPC_Z_VEL_MAX 2 + param set MPC_Z_VEL_P 0.20 +fi + +set HIL yes + +set VEHICLE_TYPE mc +set FRAME_GEOMETRY quad_x diff --git a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil index 0cc07ad34..5ec70043a 100644 --- a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil +++ b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil @@ -1,105 +1,46 @@ #!nsh # -# USB HIL start +# HIL Quadcopter + # - -echo "[HIL] HILS quadrotor + starting.." - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - - param set MC_ATTRATE_D 0.0 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.05 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 - param set MC_ATT_P 3.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_P 2.1 - param set MC_YAWRATE_D 0.0 - param set MC_YAWRATE_I 0.0 - param set MC_YAWRATE_P 0.05 - param set NAV_TAKEOFF_ALT 3.0 - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.5 - param set MPC_THR_MIN 0.1 - param set MPC_XY_D 0 - param set MPC_XY_P 0.5 - param set MPC_XY_VEL_D 0 - param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 3 - param set MPC_XY_VEL_P 0.2 - param set MPC_Z_D 0 - param set MPC_Z_P 1 - param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.1 - param set MPC_Z_VEL_MAX 2 - param set MPC_Z_VEL_P 0.20 - - param save -fi - -# Allow USB some time to come up -sleep 1 -# Tell MAVLink that this link is "fast" -mavlink start -b 230400 -d /dev/ttyACM0 - -# Create a fake HIL /dev/pwm_output interface -hil mode_pwm - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ +# Maintainers: Anton Babushkin # -param set MAV_TYPE 2 -# -# Check if we got an IO -# -if px4io start +if [ $DO_AUTOCONFIG == yes ] then - echo "IO started" -else - fmu mode_serial - echo "FMU started" + # + # Default parameters for this platform + # + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_YAWPOS_P 2.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + param set MPC_TILT_MAX 0.5 + param set MPC_THR_MAX 0.8 + param set MPC_THR_MIN 0.2 + param set MPC_XY_D 0 + param set MPC_XY_P 0.5 + param set MPC_XY_VEL_D 0 + param set MPC_XY_VEL_I 0 + param set MPC_XY_VEL_MAX 3 + param set MPC_XY_VEL_P 0.2 + param set MPC_Z_D 0 + param set MPC_Z_P 1 + param set MPC_Z_VEL_D 0 + param set MPC_Z_VEL_I 0.1 + param set MPC_Z_VEL_MAX 2 + param set MPC_Z_VEL_P 0.20 fi -# -# Start the sensors (depends on orb, px4io) -# -sh /etc/init.d/rc.sensors - -# -# Start the attitude estimator (depends on orb) -# -att_pos_estimator_ekf start - -# -# Load mixer and start controllers (depends on px4io) -# -mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix - -# -# Start position estimator -# -position_estimator_inav start - -# -# Start attitude control -# -multirotor_att_control start - -# -# Start position control -# -multirotor_pos_control start - -echo "[HIL] setup done, running" +set HIL yes +set VEHICLE_TYPE mc +set FRAME_GEOMETRY quad_+ diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index 0cfe68b27..94afe91ae 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -1,8 +1,9 @@ #!nsh - +# +# DJI Flame Wheel F330 Quadcopter +# # Maintainers: Anton Babushkin - -echo "[init] DJI F330" +# if [ $DO_AUTOCONFIG == yes ] then diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index 4a0953a2e..21b3088d3 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -1,8 +1,9 @@ #!nsh - +# +# DJI Flame Wheel F450 Quadcopter +# # Maintainers: Lorenz Meier - -echo "[init] DJI F450" +# if [ $DO_AUTOCONFIG == yes ] then diff --git a/ROMFS/px4fmu_common/init.d/4012_hk_x550 b/ROMFS/px4fmu_common/init.d/4012_hk_x550 index 396349d9c..27f73471d 100644 --- a/ROMFS/px4fmu_common/init.d/4012_hk_x550 +++ b/ROMFS/px4fmu_common/init.d/4012_hk_x550 @@ -1,8 +1,9 @@ #!nsh - +# +# HobbyKing X550 Quadcopter +# # Maintainers: Todd Stellanova - -echo "[init] PX4FMU v1, v2 with or without IO on HobbyKing X550" +# if [ $DO_AUTOCONFIG == yes ] then @@ -21,6 +22,8 @@ then param set MC_YAWRATE_P 0.08 param set MC_YAWRATE_I 0 param set MC_YAWRATE_D 0 + + # TODO add default MPC parameters fi set VEHICLE_TYPE mc diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 9da0135b3..352a916ac 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -28,7 +28,7 @@ fi if param compare SYS_AUTOSTART 1001 then - sh /etc/init.d/1001_rc_quad.hil + sh /etc/init.d/1001_rc_quad_x.hil fi if param compare SYS_AUTOSTART 1002 @@ -38,7 +38,7 @@ fi if param compare SYS_AUTOSTART 1003 then - #sh /etc/init.d/1003_rc_quad_+.hil + sh /etc/init.d/1003_rc_quad_+.hil fi if param compare SYS_AUTOSTART 1004 diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 09d66cf41..d6d6ec352 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -13,18 +13,31 @@ set logfile /fs/microsd/bootlog.txt # # Try to mount the microSD card. # -echo "[init] looking for microSD..." +echo "[init] Looking for microSD..." if mount -t vfat /dev/mmcsd0 /fs/microsd then - echo "[init] card mounted at /fs/microsd" + echo "[init] microSD card mounted at /fs/microsd" # Start playing the startup tune tone_alarm start else - echo "[init] no microSD card found" + echo "[init] No microSD card found" # Play SOS tone_alarm error fi +# +# Set default values here, can be overriden in rc.txt from SD card +# +set HIL no +set VEHICLE_TYPE none +set FRAME_GEOMETRY none +set OUTPUT_MODE none +set VEHICLE_TYPE none +set PWM_RATE none +set PWM_DISARMED none +set PWM_MIN none +set PWM_MAX none + # # Look for an init script on the microSD card. # @@ -70,27 +83,18 @@ then uorb start # - # Load microSD params + # Load parameters # - #if ramtron start - #then - # param select /ramtron/params - # if [ -f /ramtron/params ] - # then - # param load /ramtron/params - # fi - #else - param select /fs/microsd/params - if [ -f /fs/microsd/params ] + param select /fs/microsd/params + if [ -f /fs/microsd/params ] + then + if param load /fs/microsd/params then - if param load /fs/microsd/params - then - echo "[init] Parameters loaded" - else - echo "[init] Parameter file corrupt - ignoring" - fi + echo "[init] Parameters loaded" + else + echo "[init] Parameter file corrupt - ignoring" fi - #fi + fi # # Start system state indicator @@ -104,13 +108,9 @@ then blinkm systemstate fi fi - - # Use FMU PWM output by default - set OUTPUT_MODE fmu_pwm - set IO_PRESENT no # - # Upgrade PX4IO firmware + # Check if PX4IO present and update firmware if needed # if [ -f /etc/extras/px4io-v2_default.bin ] then @@ -119,15 +119,14 @@ then set io_file /etc/extras/px4io-v1_default.bin fi + set IO_PRESENT no + if px4io checkcrc $io_file then echo "[init] PX4IO CRC OK" echo "PX4IO CRC OK" >> $logfile set IO_PRESENT yes - - # If PX4IO present, use it as primary PWM output by default - set OUTPUT_MODE io_pwm else echo "[init] PX4IO CRC failure" echo "PX4IO CRC failure" >> $logfile @@ -142,9 +141,6 @@ then tone_alarm MSPAA set IO_PRESENT yes - - # If PX4IO present, use it as primary PWM output by default - set OUTPUT_MODE io_pwm else echo "[init] PX4IO restart failed" echo "PX4IO restart failed" >> $logfile @@ -164,16 +160,27 @@ then fi fi fi - - set HIL no - set FRAME_TYPE none - set PWM_RATE none - set PWM_DISARMED none - set PWM_MIN none - set PWM_MAX none + + # + # Set default output if it was not defined in rc.txt + # + if [ $OUTPUT_MODE == none ] + then + if [ $IO_PRESENT == yes ] + then + # If PX4IO present, use it as primary PWM output by default + set OUTPUT_MODE io_pwm + else + # Else use PX4FMU PWM output + set OUTPUT_MODE fmu_pwm + fi + fi set EXIT_ON_END no + # + # Set DO_AUTOCONFIG flag to use it in AUTOSTART scripts + # if param compare SYS_AUTOCONFIG 1 then set DO_AUTOCONFIG yes @@ -181,41 +188,35 @@ then set DO_AUTOCONFIG no fi - # - # Start the Commander (needs to be this early for in-air-restarts) - # - commander start - # # Set parameters and env variables for selected AUTOSTART # sh /etc/init.d/rc.autostart - # Custom configuration - if [ -f /fs/microsd/etc/rc.conf ] - then - sh /fs/microsd/etc/rc.conf - fi - - if [ $HIL == yes ] - then - set OUTPUT_MODE hil - else - # Try to get an USB console if not in HIL mode - nshterm /dev/ttyACM0 & - fi - - # - # If autoconfig parameter was set, reset it and save parameters - # - if [ $DO_AUTOCONFIG == yes ] - then - param set SYS_AUTOCONFIG 0 - param save - fi - if [ $MODE == autostart ] then + if [ $HIL == yes ] + then + set OUTPUT_MODE hil + else + # Try to get an USB console if not in HIL mode + nshterm /dev/ttyACM0 & + fi + + # + # If autoconfig parameter was set, reset it and save parameters + # + if [ $DO_AUTOCONFIG == yes ] + then + param set SYS_AUTOCONFIG 0 + param save + fi + + # + # Start the Commander (needs to be this early for in-air-restarts) + # + commander start + # # Start primary output # @@ -224,29 +225,47 @@ then echo "[init] Use PX4IO PWM as primary output" if px4io start then - echo "[init] PX4IO OK" - echo "PX4IO OK" >> $logfile + echo "[init] PX4IO started" sh /etc/init.d/rc.io else echo "[init] PX4IO start error" - echo "PX4IO start error" >> $logfile tone_alarm MNGGG fi fi if [ $OUTPUT_MODE == fmu_pwm ] then echo "[init] Use PX4FMU PWM as primary output" - fmu mode_pwm + if fmu mode_pwm + then + echo "[init] PX4FMU PWM output started" + sh /etc/init.d/rc.io + else + echo "[init] PX4FMU PWM output start error" + tone_alarm MNGGG + fi fi if [ $OUTPUT_MODE == mkblctrl ] then echo "[init] Use MKBLCTRL as primary output" - mkblctrl + if mkblctrl + then + echo "[init] MKBLCTRL started" + else + echo "[init] MKBLCTRL start error" + tone_alarm MNGGG + fi + fi if [ $OUTPUT_MODE == hil ] then echo "[init] Use HIL as primary output" - hil mode_pwm + if hil mode_pwm + then + echo "[init] HIL output started" + else + echo "[init] HIL output error" + tone_alarm MNGGG + fi fi # @@ -257,12 +276,10 @@ then echo "[init] Use PX4IO PWM as secondary output" if px4io start then - echo "[init] PX4IO OK" - echo "PX4IO OK" >> $logfile + echo "[init] PX4IO started" sh /etc/init.d/rc.io else echo "[init] PX4IO start error" - echo "PX4IO start error" >> $logfile tone_alarm MNGGG fi fi @@ -270,18 +287,24 @@ then # # MAVLink # - if [ $OUTPUT_MODE == fmu_pwm -a hw_ver compare PX4FMU_V1 ] + if [ $HIL == yes ] then - # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as PWM output - mavlink start -d /dev/ttyS0 + mavlink start -b 230400 -d /dev/ttyACM0 usleep 5000 - - # Exit from nsh to free port for mavlink - set EXIT_ON_END yes else - # Start MAVLink on default port: ttyS1 - mavlink start - usleep 5000 + if [ $OUTPUT_MODE == fmu_pwm -a hw_ver compare PX4FMU_V1 ] + then + # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as PWM output + mavlink start -d /dev/ttyS0 + usleep 5000 + + # Exit from nsh to free port for mavlink + set EXIT_ON_END yes + else + # Start MAVLink on default port: ttyS1 + mavlink start + usleep 5000 + fi fi # @@ -294,10 +317,7 @@ then then echo "[init] Start logging" sh /etc/init.d/rc.logging - fi - - if [ $HIL == no ] - then + echo "[init] Start GPS" gps start fi @@ -336,6 +356,7 @@ then if [ $VEHICLE_TYPE == none ] then echo "[init] Vehicle type: GENERIC" + attitude_estimator_ekf start position_estimator_inav start fi -- cgit v1.2.3 From f3a3d62cf94e85d69243715de4d5e0cf70d2dbfc Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 10 Jan 2014 23:14:29 +0100 Subject: Use rc.txt, config.txt, extras.txt files, minor boot messages fixes --- ROMFS/px4fmu_common/init.d/rc.fw_interface | 10 +- ROMFS/px4fmu_common/init.d/rc.mc_interface | 12 +- ROMFS/px4fmu_common/init.d/rcS | 420 +++++++++++++++-------------- 3 files changed, 232 insertions(+), 210 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_interface b/ROMFS/px4fmu_common/init.d/rc.fw_interface index f9517f422..c864e7c61 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_interface +++ b/ROMFS/px4fmu_common/init.d/rc.fw_interface @@ -1,6 +1,6 @@ #!nsh # -# Script to load fixedwing mixer and set rate/disarmed/min/max values for PWM output +# Script to configure fixedwing control interface # # @@ -12,7 +12,7 @@ param set MAV_TYPE 1 # # Load mixer # -echo "Frame geometry: ${FRAME_GEOMETRY}" -set MIXER /etc/mixers/FMU_${FRAME_GEOMETRY}.mix -echo "Loading mixer: ${MIXER}" -mixer load /dev/pwm_output ${MIXER} +echo "[init] Frame geometry: $FRAME_GEOMETRY" +set MIXER /etc/mixers/FMU_$FRAME_GEOMETRY.mix +echo "[init] Loading mixer: $MIXER" +mixer load /dev/pwm_output $MIXER diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_interface b/ROMFS/px4fmu_common/init.d/rc.mc_interface index 003aee81b..7fbcbd282 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_interface +++ b/ROMFS/px4fmu_common/init.d/rc.mc_interface @@ -1,6 +1,6 @@ #!nsh # -# Script to load multicopter mixer and set rate/disarmed/min/max values for PWM output +# Script to configure multicopter control interface # if [ $FRAME_GEOMETRY == quad_x -o $FRAME_GEOMETRY == quad_+ ] @@ -27,9 +27,9 @@ fi # # Load mixer # -echo "Frame geometry: $FRAME_GEOMETRY" -set MIXER /etc/mixers/FMU_${FRAME_GEOMETRY}.mix -echo "Loading mixer: $MIXER" +echo "[init] Frame geometry: $FRAME_GEOMETRY" +set MIXER /etc/mixers/FMU_$FRAME_GEOMETRY.mix +echo "[init] Loading mixer: $MIXER" mixer load /dev/pwm_output $MIXER if [ $OUTPUT_MODE == fmu_pwm -o $OUTPUT_MODE == io_pwm ] @@ -39,6 +39,7 @@ then # if [ $PWM_RATE != none ] then + echo "[init] Set PWM rate: $PWM_RATE" pwm rate -c $OUTPUTS -r $PWM_RATE fi @@ -47,14 +48,17 @@ then # if [ $PWM_DISARMED != none ] then + echo "[init] Set PWM disarmed: $PWM_DISARMED" pwm disarmed -c $OUTPUTS -p $PWM_DISARMED fi if [ $PWM_MIN != none ] then + echo "[init] Set PWM min: $PWM_MIN" pwm min -c $OUTPUTS -p $PWM_MIN fi if [ $PWM_MAX != none ] then + echo "[init] Set PWM max: $PWM_MAX" pwm max -c $OUTPUTS -p $PWM_MAX fi fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index d6d6ec352..576fc2d94 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -8,7 +8,10 @@ # set MODE autostart -set logfile /fs/microsd/bootlog.txt +set LOG_FILE /fs/microsd/bootlog.txt +set RC_FILE /fs/microsd/etc/rc.txt +set CONFIG_FILE /fs/microsd/etc/config.txt +set EXTRAS_FILE /fs/microsd/etc/extras.txt # # Try to mount the microSD card. @@ -25,35 +28,17 @@ else tone_alarm error fi -# -# Set default values here, can be overriden in rc.txt from SD card -# -set HIL no -set VEHICLE_TYPE none -set FRAME_GEOMETRY none -set OUTPUT_MODE none -set VEHICLE_TYPE none -set PWM_RATE none -set PWM_DISARMED none -set PWM_MIN none -set PWM_MAX none - # # Look for an init script on the microSD card. +# Disable autostart if the script found. # -# To prevent automatic startup in the current flight mode, -# the script should set MODE to some other value. -# -if [ -f /fs/microsd/etc/rc ] -then - echo "[init] reading /fs/microsd/etc/rc" - sh /fs/microsd/etc/rc -fi -# Also consider rc.txt files -if [ -f /fs/microsd/etc/rc.txt ] +if [ -f $RC_FILE ] then - echo "[init] reading /fs/microsd/etc/rc.txt" - sh /fs/microsd/etc/rc.txt + echo "[init] Executing init script: $RC_FILE" + sh $RC_FILE + set MODE custom +else + echo "[init] Init script not found: $RC_FILE" fi # if this is an APM build then there will be a rc.APM script @@ -72,15 +57,7 @@ fi if [ $MODE == autostart ] then - # - # Start CDC/ACM serial driver - # - sercon - - # - # Start the ORB (first app to start) - # - uorb start + echo "[init] AUTOSTART mode" # # Load parameters @@ -95,6 +72,16 @@ then echo "[init] Parameter file corrupt - ignoring" fi fi + + # + # Start CDC/ACM serial driver + # + sercon + + # + # Start the ORB (first app to start) + # + uorb start # # Start system state indicator @@ -114,36 +101,36 @@ then # if [ -f /etc/extras/px4io-v2_default.bin ] then - set io_file /etc/extras/px4io-v2_default.bin + set IO_FILE /etc/extras/px4io-v2_default.bin else - set io_file /etc/extras/px4io-v1_default.bin + set IO_FILE /etc/extras/px4io-v1_default.bin fi set IO_PRESENT no - if px4io checkcrc $io_file + if px4io checkcrc $IO_FILE then echo "[init] PX4IO CRC OK" - echo "PX4IO CRC OK" >> $logfile + echo "PX4IO CRC OK" >> $LOG_FILE set IO_PRESENT yes else echo "[init] PX4IO CRC failure" - echo "PX4IO CRC failure" >> $logfile + echo "PX4IO CRC failure" >> $LOG_FILE tone_alarm MBABGP - if px4io forceupdate 14662 $io_file + if px4io forceupdate 14662 $IO_FILE then usleep 500000 if px4io start then echo "[init] PX4IO restart OK" - echo "PX4IO restart OK" >> $logfile + echo "PX4IO restart OK" >> $LOG_FILE tone_alarm MSPAA set IO_PRESENT yes else echo "[init] PX4IO restart failed" - echo "PX4IO restart failed" >> $logfile + echo "PX4IO restart failed" >> $LOG_FILE if hw_ver compare PX4FMU_V2 then tone_alarm MNGGG @@ -153,27 +140,35 @@ then fi else echo "[init] PX4IO update failed" - echo "PX4IO update failed" >> $logfile + echo "PX4IO update failed" >> $LOG_FILE if hw_ver compare PX4FMU_V2 then tone_alarm MNGGG fi fi fi - + # - # Set default output if it was not defined in rc.txt + # Set default values # - if [ $OUTPUT_MODE == none ] + set HIL no + set VEHICLE_TYPE none + set FRAME_GEOMETRY none + set PWM_RATE none + set PWM_DISARMED none + set PWM_MIN none + set PWM_MAX none + + # + # Set default output + # + if [ $IO_PRESENT == yes ] then - if [ $IO_PRESENT == yes ] - then - # If PX4IO present, use it as primary PWM output by default - set OUTPUT_MODE io_pwm - else - # Else use PX4FMU PWM output - set OUTPUT_MODE fmu_pwm - fi + # If PX4IO present, use it as primary PWM output by default + set OUTPUT_MODE io_pwm + else + # Else use PX4FMU PWM output + set OUTPUT_MODE fmu_pwm fi set EXIT_ON_END no @@ -193,179 +188,202 @@ then # sh /etc/init.d/rc.autostart - if [ $MODE == autostart ] - then - if [ $HIL == yes ] - then - set OUTPUT_MODE hil - else - # Try to get an USB console if not in HIL mode - nshterm /dev/ttyACM0 & - fi - - # - # If autoconfig parameter was set, reset it and save parameters - # - if [ $DO_AUTOCONFIG == yes ] - then - param set SYS_AUTOCONFIG 0 - param save - fi + # + # Override parameters from user configuration file + # + if [ -f $CONFIG_FILE ] + then + echo "[init] Reading config: $CONFIG_FILE" + sh $CONFIG_FILE + else + echo "[init] Config file not found: $CONFIG_FILE" + fi - # - # Start the Commander (needs to be this early for in-air-restarts) - # - commander start - - # - # Start primary output - # - if [ $OUTPUT_MODE == io_pwm ] + if [ $HIL == yes ] + then + set OUTPUT_MODE hil + else + # Try to get an USB console if not in HIL mode + nshterm /dev/ttyACM0 & + fi + + # + # If autoconfig parameter was set, reset it and save parameters + # + if [ $DO_AUTOCONFIG == yes ] + then + param set SYS_AUTOCONFIG 0 + param save + fi + + # + # Start the Commander (needs to be this early for in-air-restarts) + # + commander start + + # + # Start primary output + # + if [ $OUTPUT_MODE == io_pwm ] + then + echo "[init] Use PX4IO PWM as primary output" + if px4io start then - echo "[init] Use PX4IO PWM as primary output" - if px4io start - then - echo "[init] PX4IO started" - sh /etc/init.d/rc.io - else - echo "[init] PX4IO start error" - tone_alarm MNGGG - fi + echo "[init] PX4IO started" + sh /etc/init.d/rc.io + else + echo "[init] PX4IO start error" + tone_alarm MNGGG fi - if [ $OUTPUT_MODE == fmu_pwm ] + fi + if [ $OUTPUT_MODE == fmu_pwm ] + then + echo "[init] Use PX4FMU PWM as primary output" + if fmu mode_pwm then - echo "[init] Use PX4FMU PWM as primary output" - if fmu mode_pwm - then - echo "[init] PX4FMU PWM output started" - sh /etc/init.d/rc.io - else - echo "[init] PX4FMU PWM output start error" - tone_alarm MNGGG - fi + echo "[init] PX4FMU PWM output started" + sh /etc/init.d/rc.io + else + echo "[init] PX4FMU PWM output start error" + tone_alarm MNGGG fi - if [ $OUTPUT_MODE == mkblctrl ] - then - echo "[init] Use MKBLCTRL as primary output" - if mkblctrl - then - echo "[init] MKBLCTRL started" - else - echo "[init] MKBLCTRL start error" - tone_alarm MNGGG - fi - - fi - if [ $OUTPUT_MODE == hil ] + fi + if [ $OUTPUT_MODE == mkblctrl ] + then + echo "[init] Use MKBLCTRL as primary output" + if mkblctrl then - echo "[init] Use HIL as primary output" - if hil mode_pwm - then - echo "[init] HIL output started" - else - echo "[init] HIL output error" - tone_alarm MNGGG - fi + echo "[init] MKBLCTRL started" + else + echo "[init] MKBLCTRL start error" + tone_alarm MNGGG fi - # - # Start PX4IO as secondary output if needed - # - if [ $IO_PRESENT == yes -a $OUTPUT_MODE != io_pwm ] + fi + if [ $OUTPUT_MODE == hil ] + then + echo "[init] Use HIL as primary output" + if hil mode_pwm then - echo "[init] Use PX4IO PWM as secondary output" - if px4io start - then - echo "[init] PX4IO started" - sh /etc/init.d/rc.io - else - echo "[init] PX4IO start error" - tone_alarm MNGGG - fi + echo "[init] HIL output started" + else + echo "[init] HIL output error" + tone_alarm MNGGG fi - - # - # MAVLink - # - if [ $HIL == yes ] + fi + + # + # Start PX4IO as secondary output if needed + # + if [ $IO_PRESENT == yes -a $OUTPUT_MODE != io_pwm ] + then + echo "[init] Use PX4IO PWM as secondary output" + if px4io start then - mavlink start -b 230400 -d /dev/ttyACM0 - usleep 5000 + echo "[init] PX4IO started" + sh /etc/init.d/rc.io else - if [ $OUTPUT_MODE == fmu_pwm -a hw_ver compare PX4FMU_V1 ] - then - # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as PWM output - mavlink start -d /dev/ttyS0 - usleep 5000 - - # Exit from nsh to free port for mavlink - set EXIT_ON_END yes - else - # Start MAVLink on default port: ttyS1 - mavlink start - usleep 5000 - fi + echo "[init] PX4IO start error" + tone_alarm MNGGG fi - - # - # Sensors, Logging, GPS - # - echo "[init] Start sensors" - sh /etc/init.d/rc.sensors + fi - if [ $HIL == no ] + # + # MAVLink + # + if [ $HIL == yes ] + then + mavlink start -b 230400 -d /dev/ttyACM0 + usleep 5000 + else + if [ $OUTPUT_MODE == fmu_pwm -a hw_ver compare PX4FMU_V1 ] then - echo "[init] Start logging" - sh /etc/init.d/rc.logging + # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as PWM output + mavlink start -d /dev/ttyS0 + usleep 5000 - echo "[init] Start GPS" - gps start + # Exit from nsh to free port for mavlink + set EXIT_ON_END yes + else + # Start MAVLink on default port: ttyS1 + mavlink start + usleep 5000 fi + fi + + # + # Sensors, Logging, GPS + # + echo "[init] Start sensors" + sh /etc/init.d/rc.sensors + + if [ $HIL == no ] + then + echo "[init] Start logging" + sh /etc/init.d/rc.logging - # - # Fixed wing setup - # - if [ $VEHICLE_TYPE == fw ] - then - echo "[init] Vehicle type: FIXED WING" - - # Load mixer and configure outputs - sh /etc/init.d/rc.fw_interface - - # Start standard fixedwing apps - sh /etc/init.d/rc.mc_apps - fi + echo "[init] Start GPS" + gps start + fi - # - # Multicopters setup - # - if [ $VEHICLE_TYPE == mc ] + # + # Fixed wing setup + # + if [ $VEHICLE_TYPE == fw ] + then + echo "[init] Vehicle type: FIXED WING" + + if [ $FRAME_GEOMETRY == none ] then - echo "[init] Vehicle type: MULTICOPTER" - - # Load mixer and configure outputs - sh /etc/init.d/rc.mc_interface - - # Start standard multicopter apps - sh /etc/init.d/rc.mc_apps + # Set default frame geometry for fixed wing + set FRAME_GEOMETRY AERT fi + + # Load mixer and configure outputs + sh /etc/init.d/rc.fw_interface + + # Start standard fixedwing apps + sh /etc/init.d/rc.mc_apps + fi - # - # Generic setup (autostart ID not found) - # - if [ $VEHICLE_TYPE == none ] + # + # Multicopters setup + # + if [ $VEHICLE_TYPE == mc ] + then + echo "[init] Vehicle type: MULTICOPTER" + + if [ $FRAME_GEOMETRY == none ] then - echo "[init] Vehicle type: GENERIC" - - attitude_estimator_ekf start - position_estimator_inav start + # Set default frame geometry for multicopter + set FRAME_GEOMETRY quad_x fi + + # Load mixer and configure outputs + sh /etc/init.d/rc.mc_interface + + # Start standard multicopter apps + sh /etc/init.d/rc.mc_apps fi - # Start any custom extensions - if [ -f /fs/microsd/etc/rc.local ] + # + # Generic setup (autostart ID not found) + # + if [ $VEHICLE_TYPE == none ] then - sh /fs/microsd/etc/rc.local + echo "[init] Vehicle type: GENERIC" + + attitude_estimator_ekf start + position_estimator_inav start + fi + + # Start any custom addons + if [ -f $EXTRAS_FILE ] + then + echo "[init] Starting addons script: $EXTRAS_FILE" + sh $EXTRAS_FILE + else + echo "[init] Addons script not found: $EXTRAS_FILE" fi if [ $EXIT_ON_END == yes ] -- cgit v1.2.3 From 2e1199299219baf487b4c31ebf4453ffaf971fbf Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 10 Jan 2014 23:41:03 +0100 Subject: Don’t try to find autostart script if SYS_AUTOSTART = 0 MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ROMFS/px4fmu_common/init.d/rcS | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 576fc2d94..178ed1812 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -186,7 +186,12 @@ then # # Set parameters and env variables for selected AUTOSTART # - sh /etc/init.d/rc.autostart + if param compare SYS_AUTOSTART 0 + then + echo "[init] Don't try to find autostart script" + else + sh /etc/init.d/rc.autostart + fi # # Override parameters from user configuration file -- cgit v1.2.3 From e301bdcf99271a6c95bbe6b83ab7b24ba682b211 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 11 Jan 2014 00:34:46 +0100 Subject: Autostart: some fixed wing setups added --- ROMFS/px4fmu_common/init.d/10015_tbs_discovery | 6 +- ROMFS/px4fmu_common/init.d/2100_mpx_easystar | 9 ++- ROMFS/px4fmu_common/init.d/3031_io_phantom | 85 -------------------------- ROMFS/px4fmu_common/init.d/3031_phantom | 44 +++++++++++++ ROMFS/px4fmu_common/init.d/3033_io_wingwing | 84 ------------------------- ROMFS/px4fmu_common/init.d/3033_wingwing | 43 +++++++++++++ ROMFS/px4fmu_common/init.d/3034_fx79 | 43 +++++++++++++ ROMFS/px4fmu_common/init.d/3034_io_fx79 | 84 ------------------------- ROMFS/px4fmu_common/init.d/rc.autostart | 14 ++--- 9 files changed, 145 insertions(+), 267 deletions(-) delete mode 100644 ROMFS/px4fmu_common/init.d/3031_io_phantom create mode 100644 ROMFS/px4fmu_common/init.d/3031_phantom delete mode 100644 ROMFS/px4fmu_common/init.d/3033_io_wingwing create mode 100644 ROMFS/px4fmu_common/init.d/3033_wingwing create mode 100644 ROMFS/px4fmu_common/init.d/3034_fx79 delete mode 100644 ROMFS/px4fmu_common/init.d/3034_io_fx79 diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery index 24f1099d3..53140caff 100644 --- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -24,9 +24,7 @@ then param set MC_YAWRATE_D 0.0 fi -set FRAME_TYPE mc +set VEHICLE_TYPE mc set FRAME_GEOMETRY quad_w + set PWM_RATE 400 -set PWM_DISARMED 900 -set PWM_MIN 1100 -set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/2100_mpx_easystar b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar index 9bf01c60c..43f911a78 100644 --- a/ROMFS/px4fmu_common/init.d/2100_mpx_easystar +++ b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar @@ -1,6 +1,9 @@ #!nsh - -echo "[init] EasyStar" +# +# MPX EasyStar Plane +# +# Maintainers: ??? +# if [ $DO_AUTOCONFIG == yes ] then @@ -32,5 +35,5 @@ then param set RC_SCALE_PITCH 1.0 fi -set FRAME_TYPE fw +set VEHICLE_TYPE fw set FRAME_GEOMETRY RET diff --git a/ROMFS/px4fmu_common/init.d/3031_io_phantom b/ROMFS/px4fmu_common/init.d/3031_io_phantom deleted file mode 100644 index 0cf6ee39a..000000000 --- a/ROMFS/px4fmu_common/init.d/3031_io_phantom +++ /dev/null @@ -1,85 +0,0 @@ -#!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on Phantom FPV" - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set FW_AIRSPD_MIN 11.4 - param set FW_AIRSPD_TRIM 14 - param set FW_AIRSPD_MAX 22 - param set FW_L1_PERIOD 15 - param set FW_P_D 0 - param set FW_P_I 0 - param set FW_P_IMAX 15 - param set FW_P_LIM_MAX 45 - param set FW_P_LIM_MIN -45 - param set FW_P_P 60 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 2 - param set FW_R_D 0 - param set FW_R_I 5 - param set FW_R_IMAX 15 - param set FW_R_P 80 - param set FW_R_RMAX 60 - param set FW_THR_CRUISE 0.8 - param set FW_THR_LND_MAX 0 - param set FW_THR_MAX 1 - param set FW_THR_MIN 0.5 - param set FW_T_SINK_MAX 5.0 - param set FW_T_SINK_MIN 2.0 - param set FW_Y_ROLLFF 1.0 - param set RC_SCALE_ROLL 0.6 - param set RC_SCALE_PITCH 0.6 - param set TRIM_PITCH 0.1 - - param set SYS_AUTOCONFIG 0 - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - - sh /etc/init.d/rc.io - # Limit to 100 Hz updates and (implicit) 50 Hz PWM - px4io limit 100 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix -else - echo "Using /etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing diff --git a/ROMFS/px4fmu_common/init.d/3031_phantom b/ROMFS/px4fmu_common/init.d/3031_phantom new file mode 100644 index 000000000..29af48ec6 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/3031_phantom @@ -0,0 +1,44 @@ +#!nsh +# +# Phantom FPV Flying Wing +# +# Maintainers: Simon Wilks +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set FW_AIRSPD_MIN 11.4 + param set FW_AIRSPD_TRIM 14 + param set FW_AIRSPD_MAX 22 + param set FW_L1_PERIOD 15 + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 15 + param set FW_P_LIM_MAX 45 + param set FW_P_LIM_MIN -45 + param set FW_P_P 60 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 2 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 15 + param set FW_R_P 80 + param set FW_R_RMAX 60 + param set FW_THR_CRUISE 0.8 + param set FW_THR_LND_MAX 0 + param set FW_THR_MAX 1 + param set FW_THR_MIN 0.5 + param set FW_T_SINK_MAX 5.0 + param set FW_T_SINK_MIN 2.0 + param set FW_Y_ROLLFF 1.0 + param set RC_SCALE_ROLL 0.6 + param set RC_SCALE_PITCH 0.6 + param set TRIM_PITCH 0.1 +fi + +set VEHICLE_TYPE fw +set FRAME_GEOMETRY Q diff --git a/ROMFS/px4fmu_common/init.d/3033_io_wingwing b/ROMFS/px4fmu_common/init.d/3033_io_wingwing deleted file mode 100644 index 82ff425e6..000000000 --- a/ROMFS/px4fmu_common/init.d/3033_io_wingwing +++ /dev/null @@ -1,84 +0,0 @@ -#!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on the Wing Wing (aka Z-84)" - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set FW_AIRSPD_MIN 7 - param set FW_AIRSPD_TRIM 9 - param set FW_AIRSPD_MAX 14 - param set FW_L1_PERIOD 10 - param set FW_P_D 0 - param set FW_P_I 0 - param set FW_P_IMAX 20 - param set FW_P_LIM_MAX 30 - param set FW_P_LIM_MIN -20 - param set FW_P_P 30 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 2 - param set FW_R_D 0 - param set FW_R_I 5 - param set FW_R_IMAX 20 - param set FW_R_P 60 - param set FW_R_RMAX 60 - param set FW_THR_CRUISE 0.65 - param set FW_THR_MAX 0.7 - param set FW_THR_MIN 0 - param set FW_T_SINK_MAX 5 - param set FW_T_SINK_MIN 2 - param set FW_T_TIME_CONST 9 - param set FW_Y_ROLLFF 2.0 - param set RC_SCALE_ROLL 1.0 - param set RC_SCALE_PITCH 1.0 - - param set SYS_AUTOCONFIG 0 - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - - sh /etc/init.d/rc.io - # Limit to 100 Hz updates and (implicit) 50 Hz PWM - px4io limit 100 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix -else - echo "Using /etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing new file mode 100644 index 000000000..e0340a8d9 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/3033_wingwing @@ -0,0 +1,43 @@ +#!nsh +# +# Wing Wing (aka Z-84) Flying Wing +# +# Maintainers: Simon Wilks +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set FW_AIRSPD_MIN 7 + param set FW_AIRSPD_TRIM 9 + param set FW_AIRSPD_MAX 14 + param set FW_L1_PERIOD 10 + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 20 + param set FW_P_LIM_MAX 30 + param set FW_P_LIM_MIN -20 + param set FW_P_P 30 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 2 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 20 + param set FW_R_P 60 + param set FW_R_RMAX 60 + param set FW_THR_CRUISE 0.65 + param set FW_THR_MAX 0.7 + param set FW_THR_MIN 0 + param set FW_T_SINK_MAX 5 + param set FW_T_SINK_MIN 2 + param set FW_T_TIME_CONST 9 + param set FW_Y_ROLLFF 2.0 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 +fi + +set VEHICLE_TYPE fw +set FRAME_GEOMETRY Q diff --git a/ROMFS/px4fmu_common/init.d/3034_fx79 b/ROMFS/px4fmu_common/init.d/3034_fx79 new file mode 100644 index 000000000..c4dab7ba6 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/3034_fx79 @@ -0,0 +1,43 @@ +#!nsh +# +# FX-79 Buffalo Flying Wing +# +# Maintainers: Simon Wilks +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set FW_AIRSPD_MAX 20 + param set FW_AIRSPD_TRIM 12 + param set FW_AIRSPD_MIN 15 + param set FW_L1_PERIOD 12 + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 15 + param set FW_P_LIM_MAX 50 + param set FW_P_LIM_MIN -50 + param set FW_P_P 60 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 1.1 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 20 + param set FW_R_P 80 + param set FW_R_RMAX 100 + param set FW_THR_CRUISE 0.75 + param set FW_THR_MAX 1 + param set FW_THR_MIN 0 + param set FW_T_SINK_MAX 5.0 + param set FW_T_SINK_MIN 4.0 + param set FW_T_TIME_CONST 9 + param set FW_Y_ROLLFF 1.1 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 +fi + +set VEHICLE_TYPE fw +set FRAME_GEOMETRY Q diff --git a/ROMFS/px4fmu_common/init.d/3034_io_fx79 b/ROMFS/px4fmu_common/init.d/3034_io_fx79 deleted file mode 100644 index 759c17bb4..000000000 --- a/ROMFS/px4fmu_common/init.d/3034_io_fx79 +++ /dev/null @@ -1,84 +0,0 @@ -#!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on FX-79 Buffalo" - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set FW_AIRSPD_MAX 20 - param set FW_AIRSPD_TRIM 12 - param set FW_AIRSPD_MIN 15 - param set FW_L1_PERIOD 12 - param set FW_P_D 0 - param set FW_P_I 0 - param set FW_P_IMAX 15 - param set FW_P_LIM_MAX 50 - param set FW_P_LIM_MIN -50 - param set FW_P_P 60 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 1.1 - param set FW_R_D 0 - param set FW_R_I 5 - param set FW_R_IMAX 20 - param set FW_R_P 80 - param set FW_R_RMAX 100 - param set FW_THR_CRUISE 0.75 - param set FW_THR_MAX 1 - param set FW_THR_MIN 0 - param set FW_T_SINK_MAX 5.0 - param set FW_T_SINK_MIN 4.0 - param set FW_T_TIME_CONST 9 - param set FW_Y_ROLLFF 1.1 - param set RC_SCALE_ROLL 1.0 - param set RC_SCALE_PITCH 1.0 - - param set SYS_AUTOCONFIG 0 - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - - sh /etc/init.d/rc.io - # Limit to 100 Hz updates and (implicit) 50 Hz PWM - px4io limit 100 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_FX79.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_FX79.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_FX79.mix -else - echo "Using /etc/mixers/FMU_FX79.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_FX79.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 352a916ac..69e88fcd0 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -77,24 +77,24 @@ then #sh /etc/init.d/3030_io_camflyer fi -if param compare SYS_AUTOSTART 3031 31 +if param compare SYS_AUTOSTART 3031 then - #sh /etc/init.d/3031_io_phantom + sh /etc/init.d/3031_phantom fi -if param compare SYS_AUTOSTART 3032 32 +if param compare SYS_AUTOSTART 3032 then #sh /etc/init.d/3032_skywalker_x5 fi -if param compare SYS_AUTOSTART 3033 33 +if param compare SYS_AUTOSTART 3033 then - #sh /etc/init.d/3033_io_wingwing + sh /etc/init.d/3033_wingwing fi -if param compare SYS_AUTOSTART 3034 34 +if param compare SYS_AUTOSTART 3034 then - #sh /etc/init.d/3034_io_fx79 + sh /etc/init.d/3034_fx79 fi # -- cgit v1.2.3 From 3f6f26e5f6bb89056a0b07b4761fdf81f0f8491a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 11 Jan 2014 00:40:19 +0100 Subject: Autostart for fixed wing bug fixed --- 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 178ed1812..c92876f26 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -348,7 +348,7 @@ then sh /etc/init.d/rc.fw_interface # Start standard fixedwing apps - sh /etc/init.d/rc.mc_apps + sh /etc/init.d/rc.fw_apps fi # -- cgit v1.2.3 From 95c99618f9f22ba08b4826a98c32f995ea3bcf17 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 11 Jan 2014 11:26:55 +0100 Subject: Autostart: mkblctrl mixer loading fixed --- ROMFS/px4fmu_common/init.d/rc.mc_interface | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_interface b/ROMFS/px4fmu_common/init.d/rc.mc_interface index 7fbcbd282..401ff775f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_interface +++ b/ROMFS/px4fmu_common/init.d/rc.mc_interface @@ -30,7 +30,13 @@ fi echo "[init] Frame geometry: $FRAME_GEOMETRY" set MIXER /etc/mixers/FMU_$FRAME_GEOMETRY.mix echo "[init] Loading mixer: $MIXER" -mixer load /dev/pwm_output $MIXER +if [ $OUTPUT_MODE == mkblctrl ] +then + set OUTPUT_DEV /dev/mkblctrl +else + set OUTPUT_DEV /dev/pwm_output +fi +mixer load $OUTPUT_DEV $MIXER if [ $OUTPUT_MODE == fmu_pwm -o $OUTPUT_MODE == io_pwm ] then -- cgit v1.2.3 From 41add86164e15d553a5b1d2d2f9d55d964ca4ebe Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 11 Jan 2014 11:34:18 +0100 Subject: Autostart: standalone FMUv1 setup fixed --- ROMFS/px4fmu_common/init.d/rcS | 1 - 1 file changed, 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index c92876f26..f8796bb08 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -247,7 +247,6 @@ then if fmu mode_pwm then echo "[init] PX4FMU PWM output started" - sh /etc/init.d/rc.io else echo "[init] PX4FMU PWM output start error" tone_alarm MNGGG -- cgit v1.2.3 From 4dd5c13b98f57e490274d7b3fb6d297e18e79853 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 11 Jan 2014 17:52:29 +0100 Subject: Autostart: fixes --- ROMFS/px4fmu_common/init.d/rc.mc_interface | 2 +- ROMFS/px4fmu_common/init.d/rcS | 238 ++++++++++++++++++----------- 2 files changed, 146 insertions(+), 94 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_interface b/ROMFS/px4fmu_common/init.d/rc.mc_interface index 401ff775f..12be3a931 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_interface +++ b/ROMFS/px4fmu_common/init.d/rc.mc_interface @@ -38,7 +38,7 @@ else fi mixer load $OUTPUT_DEV $MIXER -if [ $OUTPUT_MODE == fmu_pwm -o $OUTPUT_MODE == io_pwm ] +if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == io ] then # # Set PWM output frequency diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index f8796bb08..5e13e4fa2 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -96,82 +96,20 @@ then fi fi - # - # Check if PX4IO present and update firmware if needed - # - if [ -f /etc/extras/px4io-v2_default.bin ] - then - set IO_FILE /etc/extras/px4io-v2_default.bin - else - set IO_FILE /etc/extras/px4io-v1_default.bin - fi - - set IO_PRESENT no - - if px4io checkcrc $IO_FILE - then - echo "[init] PX4IO CRC OK" - echo "PX4IO CRC OK" >> $LOG_FILE - - set IO_PRESENT yes - else - echo "[init] PX4IO CRC failure" - echo "PX4IO CRC failure" >> $LOG_FILE - tone_alarm MBABGP - if px4io forceupdate 14662 $IO_FILE - then - usleep 500000 - if px4io start - then - echo "[init] PX4IO restart OK" - echo "PX4IO restart OK" >> $LOG_FILE - tone_alarm MSPAA - - set IO_PRESENT yes - else - echo "[init] PX4IO restart failed" - echo "PX4IO restart failed" >> $LOG_FILE - if hw_ver compare PX4FMU_V2 - then - tone_alarm MNGGG - sleep 10 - reboot - fi - fi - else - echo "[init] PX4IO update failed" - echo "PX4IO update failed" >> $LOG_FILE - if hw_ver compare PX4FMU_V2 - then - tone_alarm MNGGG - fi - fi - fi - # # Set default values # set HIL no set VEHICLE_TYPE none set FRAME_GEOMETRY none + set USE_IO yes + set OUTPUT_MODE none set PWM_RATE none set PWM_DISARMED none set PWM_MIN none set PWM_MAX none - - # - # Set default output - # - if [ $IO_PRESENT == yes ] - then - # If PX4IO present, use it as primary PWM output by default - set OUTPUT_MODE io_pwm - else - # Else use PX4FMU PWM output - set OUTPUT_MODE fmu_pwm - fi - - set EXIT_ON_END no + set MKBLCTRL_MODE none + set FMU_MODE pwm # # Set DO_AUTOCONFIG flag to use it in AUTOSTART scripts @@ -182,7 +120,7 @@ then else set DO_AUTOCONFIG no fi - + # # Set parameters and env variables for selected AUTOSTART # @@ -204,14 +142,6 @@ then echo "[init] Config file not found: $CONFIG_FILE" fi - if [ $HIL == yes ] - then - set OUTPUT_MODE hil - else - # Try to get an USB console if not in HIL mode - nshterm /dev/ttyACM0 & - fi - # # If autoconfig parameter was set, reset it and save parameters # @@ -220,7 +150,82 @@ then param set SYS_AUTOCONFIG 0 param save fi + + set IO_PRESENT no + + if [ $USE_IO == yes ] + then + # + # Check if PX4IO present and update firmware if needed + # + if [ -f /etc/extras/px4io-v2_default.bin ] + then + set IO_FILE /etc/extras/px4io-v2_default.bin + else + set IO_FILE /etc/extras/px4io-v1_default.bin + fi + + if px4io checkcrc $IO_FILE + then + echo "[init] PX4IO CRC OK" + echo "PX4IO CRC OK" >> $LOG_FILE + + set IO_PRESENT yes + else + echo "[init] PX4IO CRC failure" + echo "PX4IO CRC failure" >> $LOG_FILE + tone_alarm MBABGP + if px4io forceupdate 14662 $IO_FILE + then + usleep 500000 + if px4io checkcrc $IO_FILE + then + echo "[init] PX4IO CRC OK after updating" + echo "PX4IO CRC OK after updating" >> $LOG_FILE + tone_alarm MSPAA + + set IO_PRESENT yes + else + echo "[init] PX4IO update failed" + echo "PX4IO update failed" >> $LOG_FILE + fi + else + echo "[init] PX4IO update failed" + echo "PX4IO update failed" >> $LOG_FILE + fi + fi + + if [ $IO_PRESENT == no ] + then + echo "[init] ERROR: PX4IO not found, set vehicle type to NONE" + tone_alarm MNGGG + set VEHICLE_TYPE none + fi + fi + + # + # Set default output if not set + # + if [ $OUTPUT_MODE == none ] + then + if [ $IO_PRESENT == yes ] + then + # If PX4IO present, use it as primary PWM output by default + set OUTPUT_MODE io + else + # Else use PX4FMU PWM output + set OUTPUT_MODE fmu + fi + fi + if [ $HIL == yes ] + then + set OUTPUT_MODE hil + else + # Try to get an USB console if not in HIL mode + nshterm /dev/ttyACM0 & + fi + # # Start the Commander (needs to be this early for in-air-restarts) # @@ -229,7 +234,9 @@ then # # Start primary output # - if [ $OUTPUT_MODE == io_pwm ] + set TTYS1_BUSY no + + if [ $OUTPUT_MODE == io ] then echo "[init] Use PX4IO PWM as primary output" if px4io start @@ -241,21 +248,43 @@ then tone_alarm MNGGG fi fi - if [ $OUTPUT_MODE == fmu_pwm ] + if [ $OUTPUT_MODE == fmu ] then - echo "[init] Use PX4FMU PWM as primary output" - if fmu mode_pwm + echo "[init] Use FMU PWM as primary output" + if fmu mode_$FMU_MODE then - echo "[init] PX4FMU PWM output started" + echo "[init] FMU mode_$FMU_MODE started" else - echo "[init] PX4FMU PWM output start error" + echo "[init] FMU mode_$FMU_MODE start error" tone_alarm MNGGG fi + + if hw_ver compare PX4FMU_V1 + then + if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ] + then + set TTYS1_BUSY yes + fi + if [ $FMU_MODE == pwm_gpio ] + then + set TTYS1_BUSY yes + fi + fi fi if [ $OUTPUT_MODE == mkblctrl ] then echo "[init] Use MKBLCTRL as primary output" - if mkblctrl + set MKBLCTRL_ARG "" + if [ $MKBLCTRL_MODE == x ] + then + set MKBLCTRL_ARG "-mkmode x" + fi + if [ $MKBLCTRL_MODE == + ] + then + set MKBLCTRL_ARG "-mkmode +" + fi + + if mkblctrl $MKBLCTRL_ARG then echo "[init] MKBLCTRL started" else @@ -277,32 +306,55 @@ then fi # - # Start PX4IO as secondary output if needed + # Start IO or FMU for RC PPM input if needed # - if [ $IO_PRESENT == yes -a $OUTPUT_MODE != io_pwm ] + if [ $IO_PRESENT == yes ] then - echo "[init] Use PX4IO PWM as secondary output" - if px4io start + if [ $OUTPUT_MODE != io ] then - echo "[init] PX4IO started" - sh /etc/init.d/rc.io - else - echo "[init] PX4IO start error" - tone_alarm MNGGG + if px4io start + then + echo "[init] PX4IO started" + sh /etc/init.d/rc.io + else + echo "[init] PX4IO start error" + tone_alarm MNGGG + fi + fi + else + if [ $OUTPUT_MODE != fmu ] + then + if fmu mode_$FMU_MODE + then + echo "[init] FMU mode_$FMU_MODE started" + else + echo "[init] FMU mode_$FMU_MODE start error" + tone_alarm MNGGG + fi + + if hw_ver compare PX4FMU_V1 + then + if [ $FMU_MODE == pwm -o $FMU_MODE == gpio -o $FMU_MODE == pwm_gpio ] + then + set TTYS1_BUSY yes + fi + fi fi fi # # MAVLink # + set EXIT_ON_END no + if [ $HIL == yes ] then mavlink start -b 230400 -d /dev/ttyACM0 usleep 5000 else - if [ $OUTPUT_MODE == fmu_pwm -a hw_ver compare PX4FMU_V1 ] + if [ $TTYS1_BUSY == yes ] then - # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as PWM output + # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else mavlink start -d /dev/ttyS0 usleep 5000 -- cgit v1.2.3 From 3339edeae6bdb4119179eeacc34ab91c0b8aced1 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 11 Jan 2014 18:45:56 +0100 Subject: Autostart: tones cleanup --- ROMFS/px4fmu_common/init.d/rc.fw_interface | 10 ++++++++-- ROMFS/px4fmu_common/init.d/rc.mc_interface | 11 +++++++++-- ROMFS/px4fmu_common/init.d/rcS | 24 +++++++++++++----------- 3 files changed, 30 insertions(+), 15 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_interface b/ROMFS/px4fmu_common/init.d/rc.fw_interface index c864e7c61..69f3ed7f7 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_interface +++ b/ROMFS/px4fmu_common/init.d/rc.fw_interface @@ -14,5 +14,11 @@ param set MAV_TYPE 1 # echo "[init] Frame geometry: $FRAME_GEOMETRY" set MIXER /etc/mixers/FMU_$FRAME_GEOMETRY.mix -echo "[init] Loading mixer: $MIXER" -mixer load /dev/pwm_output $MIXER + +if mixer load /dev/pwm_output $MIXER +then + echo "[init] Mixer loaded: $MIXER" +else + echo "[init] Error loading mixer: $MIXER" + tone_alarm $TUNE_OUT_ERROR +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_interface b/ROMFS/px4fmu_common/init.d/rc.mc_interface index 12be3a931..6e4e4ed31 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_interface +++ b/ROMFS/px4fmu_common/init.d/rc.mc_interface @@ -29,14 +29,21 @@ fi # echo "[init] Frame geometry: $FRAME_GEOMETRY" set MIXER /etc/mixers/FMU_$FRAME_GEOMETRY.mix -echo "[init] Loading mixer: $MIXER" + if [ $OUTPUT_MODE == mkblctrl ] then set OUTPUT_DEV /dev/mkblctrl else set OUTPUT_DEV /dev/pwm_output fi -mixer load $OUTPUT_DEV $MIXER + +if mixer load $OUTPUT_DEV $MIXER +then + echo "[init] Mixer loaded: $MIXER" +else + echo "[init] Error loading mixer: $MIXER" + tone_alarm $TUNE_OUT_ERROR +fi if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == io ] then diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 5e13e4fa2..a809f2870 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -13,6 +13,7 @@ set RC_FILE /fs/microsd/etc/rc.txt set CONFIG_FILE /fs/microsd/etc/config.txt set EXTRAS_FILE /fs/microsd/etc/extras.txt +set TUNE_OUT_ERROR ML<> $LOG_FILE - tone_alarm MBABGP + + tone_alarm MLL32CP8MB + if px4io forceupdate 14662 $IO_FILE then usleep 500000 @@ -182,7 +185,7 @@ then then echo "[init] PX4IO CRC OK after updating" echo "PX4IO CRC OK after updating" >> $LOG_FILE - tone_alarm MSPAA + tone_alarm MLL8CDE set IO_PRESENT yes else @@ -197,9 +200,8 @@ then if [ $IO_PRESENT == no ] then - echo "[init] ERROR: PX4IO not found, set vehicle type to NONE" - tone_alarm MNGGG - set VEHICLE_TYPE none + echo "[init] ERROR: PX4IO not found" + tone_alarm $TUNE_OUT_ERROR fi fi @@ -245,7 +247,7 @@ then sh /etc/init.d/rc.io else echo "[init] PX4IO start error" - tone_alarm MNGGG + tone_alarm $TUNE_OUT_ERROR fi fi if [ $OUTPUT_MODE == fmu ] @@ -256,7 +258,7 @@ then echo "[init] FMU mode_$FMU_MODE started" else echo "[init] FMU mode_$FMU_MODE start error" - tone_alarm MNGGG + tone_alarm $TUNE_OUT_ERROR fi if hw_ver compare PX4FMU_V1 @@ -289,7 +291,7 @@ then echo "[init] MKBLCTRL started" else echo "[init] MKBLCTRL start error" - tone_alarm MNGGG + tone_alarm $TUNE_OUT_ERROR fi fi @@ -301,7 +303,7 @@ then echo "[init] HIL output started" else echo "[init] HIL output error" - tone_alarm MNGGG + tone_alarm $TUNE_OUT_ERROR fi fi @@ -318,7 +320,7 @@ then sh /etc/init.d/rc.io else echo "[init] PX4IO start error" - tone_alarm MNGGG + tone_alarm $TUNE_OUT_ERROR fi fi else @@ -329,7 +331,7 @@ then echo "[init] FMU mode_$FMU_MODE started" else echo "[init] FMU mode_$FMU_MODE start error" - tone_alarm MNGGG + tone_alarm $TUNE_OUT_ERROR fi if hw_ver compare PX4FMU_V1 -- cgit v1.2.3 From d079074e801a6cb590f91053e2e6d96d5113c5c5 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 12 Jan 2014 18:49:43 +0100 Subject: update the skywalker x5 script --- ROMFS/px4fmu_common/init.d/3032_skywalker_x5 | 56 +++------------------------- 1 file changed, 6 insertions(+), 50 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 index 41e041654..bda05aeb1 100644 --- a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 +++ b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 @@ -1,58 +1,14 @@ #!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on Skywalker X5" - -# -# Load default params for this platform # -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - # TODO - - param set SYS_AUTOCONFIG 0 - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 - +# Skywalker X5 Flying Wing # -# Start and configure PX4IO or FMU interface +# Maintainers: Thomas Gubler , Julian Oes # -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - - sh /etc/init.d/rc.io - # Limit to 100 Hz updates and (implicit) 50 Hz PWM - px4io limit 100 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ] +if [ $DO_AUTOCONFIG == yes ] then - echo "Using /fs/microsd/etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix -else - echo "Using /etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix + # TODO fi -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing +set VEHICLE_TYPE fw +set FRAME_GEOMETRY Q -- cgit v1.2.3 From 449ed058c3554d2317e25395a2f5ebdb21816294 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 12 Jan 2014 18:55:41 +0100 Subject: rc.fw_interface: use mixer file from sd if it exists --- ROMFS/px4fmu_common/init.d/rc.fw_interface | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_interface b/ROMFS/px4fmu_common/init.d/rc.fw_interface index 69f3ed7f7..1bf7a0e03 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_interface +++ b/ROMFS/px4fmu_common/init.d/rc.fw_interface @@ -13,7 +13,17 @@ param set MAV_TYPE 1 # Load mixer # echo "[init] Frame geometry: $FRAME_GEOMETRY" -set MIXER /etc/mixers/FMU_$FRAME_GEOMETRY.mix +set MIXERSD /fs/microsd/etc/mixers/FMU_$FRAME_GEOMETRY.mix + +#Use the mixer file from the sd-card if it exists +if [ -f $MIXERSD ] +then + set MIXER MIXERSD +else + set MIXER /etc/mixers/FMU_$FRAME_GEOMETRY.mix +fi + + if mixer load /dev/pwm_output $MIXER then -- cgit v1.2.3 From 1008d0c38320544579eb449d54c053e14568585e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 13 Jan 2014 08:03:38 +0100 Subject: fix small error in rc.fw_interface --- ROMFS/px4fmu_common/init.d/rc.fw_interface | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_interface b/ROMFS/px4fmu_common/init.d/rc.fw_interface index 1bf7a0e03..133b65218 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_interface +++ b/ROMFS/px4fmu_common/init.d/rc.fw_interface @@ -18,7 +18,7 @@ set MIXERSD /fs/microsd/etc/mixers/FMU_$FRAME_GEOMETRY.mix #Use the mixer file from the sd-card if it exists if [ -f $MIXERSD ] then - set MIXER MIXERSD + set MIXER $MIXERSD else set MIXER /etc/mixers/FMU_$FRAME_GEOMETRY.mix fi -- cgit v1.2.3 From db1ea9bf515afe927e7f1bde404210127ded0a4b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 13 Jan 2014 10:11:16 +0100 Subject: 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 bdb4a0a1c..e10b7f18d 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 3b3e56c88..bbc84ef93 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); -- cgit v1.2.3 From d5d035b9ea03775c735a938a3573772a6ed59836 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 14 Jan 2014 07:42:01 +0100 Subject: 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 e4eb1d143..000000000 --- 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 03c713987..000000000 --- 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 -- cgit v1.2.3 From ba26fc32c9ad84404fc0989fa26072b2812d87ce Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 14 Jan 2014 07:49:33 +0100 Subject: 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 000000000..e34be44e3 --- /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 686656597..b3fceceb5 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 baef9dccc..e89b6e5aa 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"); -- cgit v1.2.3 From 1e0f292566b2a80557a2727e99b74990c75c90d9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 14 Jan 2014 08:57:32 +0100 Subject: 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 c86beacca..ba26ef28f 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 d0733ec53..a269d01ab 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 41e8b95ff..4ba93fa74 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 99f26c3f3..e90312226 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 -- cgit v1.2.3 From 993f7212104814db917943bedb471990f040a532 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 14 Jan 2014 08:57:52 +0100 Subject: 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 e34be44e3..000000000 --- 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 2aed80e01..000000000 --- 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 07f3945be..000000000 --- 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 -- cgit v1.2.3 From 33b84186e3492660007c5b46c5e988e8acef60fa Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 14 Jan 2014 08:58:30 +0100 Subject: 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 4972e6914..264d911f3 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 b28d318d7..4b1b0ed0b 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 } -- cgit v1.2.3 From 202e89de911a8acddcec400b0c2956f5590d8bc8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 14 Jan 2014 08:58:58 +0100 Subject: 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 e89b6e5aa..41da63750 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]; -- cgit v1.2.3 From d1b2186806e0b9e32808a04f6c85d26a703c596e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 14 Jan 2014 21:59:48 +0100 Subject: Autostart 3DR Iris updated --- ROMFS/px4fmu_common/init.d/10016_3dr_iris | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index b8fc5e606..6740e2d94 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -38,9 +38,16 @@ then param set MPC_Z_VEL_I 0.1 param set MPC_Z_VEL_MAX 2 param set MPC_Z_VEL_P 0.20 + + param set BAT_V_SCALING 0.00989 + param set BAT_C_SCALING 0.0124 fi set VEHICLE_TYPE mc set FRAME_GEOMETRY quad_w set PWM_RATE 400 + +set PWM_DISARMED 900 +set PWM_MIN 1000 +set PWM_MAX 2000 -- cgit v1.2.3 From a8d362de13b23a2523dc69d582c68fe672ac236d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 15 Jan 2014 00:02:57 +0100 Subject: Autostart: use MIXER instead of FRAME_GEOMETRY --- ROMFS/px4fmu_common/init.d/10015_tbs_discovery | 3 +- ROMFS/px4fmu_common/init.d/10016_3dr_iris | 4 +- ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil | 2 +- ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil | 2 +- ROMFS/px4fmu_common/init.d/2100_mpx_easystar | 2 +- ROMFS/px4fmu_common/init.d/3031_phantom | 2 +- ROMFS/px4fmu_common/init.d/3032_skywalker_x5 | 2 +- ROMFS/px4fmu_common/init.d/3033_wingwing | 2 +- ROMFS/px4fmu_common/init.d/3034_fx79 | 2 +- ROMFS/px4fmu_common/init.d/4010_dji_f330 | 4 +- ROMFS/px4fmu_common/init.d/4011_dji_f450 | 4 +- ROMFS/px4fmu_common/init.d/4012_hk_x550 | 3 +- ROMFS/px4fmu_common/init.d/rc.fw_interface | 34 ------- ROMFS/px4fmu_common/init.d/rc.interface | 72 ++++++++++++++ ROMFS/px4fmu_common/init.d/rc.io | 10 +- ROMFS/px4fmu_common/init.d/rc.mc_interface | 77 --------------- ROMFS/px4fmu_common/init.d/rc.sensors | 21 ++-- ROMFS/px4fmu_common/init.d/rcS | 131 +++++++++++++++++-------- 18 files changed, 195 insertions(+), 182 deletions(-) delete mode 100644 ROMFS/px4fmu_common/init.d/rc.fw_interface create mode 100644 ROMFS/px4fmu_common/init.d/rc.interface delete mode 100644 ROMFS/px4fmu_common/init.d/rc.mc_interface diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery index 53140caff..63798bb3c 100644 --- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -25,6 +25,7 @@ then fi set VEHICLE_TYPE mc -set FRAME_GEOMETRY quad_w +set MIXER FMU_quad_w +set PWM_OUTPUTS 1234 set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index 6740e2d94..67c24fab3 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -44,10 +44,10 @@ then fi set VEHICLE_TYPE mc -set FRAME_GEOMETRY quad_w +set MIXER FMU_quad_w +set PWM_OUTPUTS 1234 set PWM_RATE 400 - set PWM_DISARMED 900 set PWM_MIN 1000 set PWM_MAX 2000 diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil index c5b92d7d4..8c0797d7c 100644 --- a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil @@ -43,4 +43,4 @@ fi set HIL yes set VEHICLE_TYPE mc -set FRAME_GEOMETRY quad_x +set MIXER FMU_quad_x diff --git a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil index 5ec70043a..bce3015fc 100644 --- a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil +++ b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil @@ -43,4 +43,4 @@ fi set HIL yes set VEHICLE_TYPE mc -set FRAME_GEOMETRY quad_+ +set MIXER FMU_quad_+ diff --git a/ROMFS/px4fmu_common/init.d/2100_mpx_easystar b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar index 43f911a78..0e5bf60d6 100644 --- a/ROMFS/px4fmu_common/init.d/2100_mpx_easystar +++ b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar @@ -36,4 +36,4 @@ then fi set VEHICLE_TYPE fw -set FRAME_GEOMETRY RET +set MIXER FMU_RET diff --git a/ROMFS/px4fmu_common/init.d/3031_phantom b/ROMFS/px4fmu_common/init.d/3031_phantom index 29af48ec6..4ebbe9c61 100644 --- a/ROMFS/px4fmu_common/init.d/3031_phantom +++ b/ROMFS/px4fmu_common/init.d/3031_phantom @@ -41,4 +41,4 @@ then fi set VEHICLE_TYPE fw -set FRAME_GEOMETRY Q +set MIXER FMU_Q diff --git a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 index bda05aeb1..03f282237 100644 --- a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 +++ b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 @@ -11,4 +11,4 @@ then fi set VEHICLE_TYPE fw -set FRAME_GEOMETRY Q +set MIXER FMU_Q diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing index e0340a8d9..e53763278 100644 --- a/ROMFS/px4fmu_common/init.d/3033_wingwing +++ b/ROMFS/px4fmu_common/init.d/3033_wingwing @@ -40,4 +40,4 @@ then fi set VEHICLE_TYPE fw -set FRAME_GEOMETRY Q +set MIXER FMU_Q diff --git a/ROMFS/px4fmu_common/init.d/3034_fx79 b/ROMFS/px4fmu_common/init.d/3034_fx79 index c4dab7ba6..8d179d1fd 100644 --- a/ROMFS/px4fmu_common/init.d/3034_fx79 +++ b/ROMFS/px4fmu_common/init.d/3034_fx79 @@ -40,4 +40,4 @@ then fi set VEHICLE_TYPE fw -set FRAME_GEOMETRY Q +set MIXER FMU_FX79 diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index 94afe91ae..ab1db94d0 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -41,10 +41,10 @@ then fi set VEHICLE_TYPE mc -set FRAME_GEOMETRY quad_x +set MIXER FMU_quad_x +set PWM_OUTPUTS 1234 set PWM_RATE 400 - # DJI ESC range set PWM_DISARMED 900 set PWM_MIN 1200 diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index 21b3088d3..299771c1d 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -27,10 +27,10 @@ then fi set VEHICLE_TYPE mc -set FRAME_GEOMETRY quad_x +set MIXER FMU_quad_x +set PWM_OUTPUTS 1234 set PWM_RATE 400 - # DJI ESC range set PWM_DISARMED 900 set PWM_MIN 1200 diff --git a/ROMFS/px4fmu_common/init.d/4012_hk_x550 b/ROMFS/px4fmu_common/init.d/4012_hk_x550 index 27f73471d..7e020cf59 100644 --- a/ROMFS/px4fmu_common/init.d/4012_hk_x550 +++ b/ROMFS/px4fmu_common/init.d/4012_hk_x550 @@ -27,6 +27,7 @@ then fi set VEHICLE_TYPE mc -set FRAME_GEOMETRY quad_x +set MIXER FMU_quad_x +set PWM_OUTPUTS 1234 set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_interface b/ROMFS/px4fmu_common/init.d/rc.fw_interface deleted file mode 100644 index 133b65218..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.fw_interface +++ /dev/null @@ -1,34 +0,0 @@ -#!nsh -# -# Script to configure fixedwing control interface -# - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 - -# -# Load mixer -# -echo "[init] Frame geometry: $FRAME_GEOMETRY" -set MIXERSD /fs/microsd/etc/mixers/FMU_$FRAME_GEOMETRY.mix - -#Use the mixer file from the sd-card if it exists -if [ -f $MIXERSD ] -then - set MIXER $MIXERSD -else - set MIXER /etc/mixers/FMU_$FRAME_GEOMETRY.mix -fi - - - -if mixer load /dev/pwm_output $MIXER -then - echo "[init] Mixer loaded: $MIXER" -else - echo "[init] Error loading mixer: $MIXER" - tone_alarm $TUNE_OUT_ERROR -fi diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface new file mode 100644 index 000000000..928d3aeeb --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -0,0 +1,72 @@ +#!nsh +# +# Script to configure control interface +# + +if [ $MIXER != none ] +then + # + # Load mixer + # + set MIXERSD /fs/microsd/etc/mixers/$MIXER.mix + + #Use the mixer file from the SD-card if it exists + if [ -f $MIXERSD ] + then + set MIXER_FILE $MIXERSD + else + set MIXER_FILE /etc/mixers/$MIXER.mix + fi + + if [ $OUTPUT_MODE == mkblctrl ] + then + set OUTPUT_DEV /dev/mkblctrl + else + set OUTPUT_DEV /dev/pwm_output + fi + + if mixer load $OUTPUT_DEV $MIXER_FILE + then + echo "[init] Mixer loaded: $MIXER_FILE" + else + echo "[init] Error loading mixer: $MIXER_FILE" + tone_alarm $TUNE_OUT_ERROR + fi +else + echo "[init] Mixer not defined + tone_alarm $TUNE_OUT_ERROR +fi + +if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == io ] +then + if [ $PWM_OUTPUTS != none ] + then + # + # Set PWM output frequency + # + if [ $PWM_RATE != none ] + then + echo "[init] Set PWM rate: $PWM_RATE" + pwm rate -c $PWM_OUTPUTS -r $PWM_RATE + fi + + # + # Set disarmed, min and max PWM values + # + if [ $PWM_DISARMED != none ] + then + echo "[init] Set PWM disarmed: $PWM_DISARMED" + pwm disarmed -c $PWM_OUTPUTS -p $PWM_DISARMED + fi + if [ $PWM_MIN != none ] + then + echo "[init] Set PWM min: $PWM_MIN" + pwm min -c $PWM_OUTPUTS -p $PWM_MIN + fi + if [ $PWM_MAX != none ] + then + echo "[init] Set PWM max: $PWM_MAX" + pwm max -c $PWM_OUTPUTS -p $PWM_MAX + fi + fi +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.io b/ROMFS/px4fmu_common/init.d/rc.io index 5a010cc9b..c9d964f8e 100644 --- a/ROMFS/px4fmu_common/init.d/rc.io +++ b/ROMFS/px4fmu_common/init.d/rc.io @@ -9,11 +9,13 @@ px4io recovery # -# Adjust px4io topic limiting +# Adjust PX4IO update rate limit # +set PX4IO_LIMIT 400 if hw_ver compare PX4FMU_V1 then - px4io limit 200 -else - px4io limit 400 + set PX4IO_LIMIT 200 fi + +echo "[init] Set PX4IO update rate limit: $PX4IO_LIMIT Hz" +px4io limit $PX4IO_LIMIT diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_interface b/ROMFS/px4fmu_common/init.d/rc.mc_interface deleted file mode 100644 index 6e4e4ed31..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.mc_interface +++ /dev/null @@ -1,77 +0,0 @@ -#!nsh -# -# Script to configure multicopter control interface -# - -if [ $FRAME_GEOMETRY == quad_x -o $FRAME_GEOMETRY == quad_+ ] -then - set OUTPUTS 1234 - param set MAV_TYPE 2 -fi -if [ $FRAME_GEOMETRY == quad_w -o $FRAME_GEOMETRY == quad_v ] -then - set OUTPUTS 1234 - param set MAV_TYPE 2 -fi -if [ $FRAME_GEOMETRY == hex_x -o $FRAME_GEOMETRY == hex_+ ] -then - set OUTPUTS 123456 - param set MAV_TYPE 13 -fi -if [ $FRAME_GEOMETRY == octo_x -o $FRAME_GEOMETRY == octo_+ ] -then - set OUTPUTS 12345678 - param set MAV_TYPE 14 -fi - -# -# Load mixer -# -echo "[init] Frame geometry: $FRAME_GEOMETRY" -set MIXER /etc/mixers/FMU_$FRAME_GEOMETRY.mix - -if [ $OUTPUT_MODE == mkblctrl ] -then - set OUTPUT_DEV /dev/mkblctrl -else - set OUTPUT_DEV /dev/pwm_output -fi - -if mixer load $OUTPUT_DEV $MIXER -then - echo "[init] Mixer loaded: $MIXER" -else - echo "[init] Error loading mixer: $MIXER" - tone_alarm $TUNE_OUT_ERROR -fi - -if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == io ] -then - # - # Set PWM output frequency - # - if [ $PWM_RATE != none ] - then - echo "[init] Set PWM rate: $PWM_RATE" - pwm rate -c $OUTPUTS -r $PWM_RATE - fi - - # - # Set disarmed, min and max PWM values - # - if [ $PWM_DISARMED != none ] - then - echo "[init] Set PWM disarmed: $PWM_DISARMED" - pwm disarmed -c $OUTPUTS -p $PWM_DISARMED - fi - if [ $PWM_MIN != none ] - then - echo "[init] Set PWM min: $PWM_MIN" - pwm min -c $OUTPUTS -p $PWM_MIN - fi - if [ $PWM_MAX != none ] - then - echo "[init] Set PWM max: $PWM_MAX" - pwm max -c $OUTPUTS -p $PWM_MAX - fi -fi diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index a2517135f..badbf92c3 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -10,39 +10,42 @@ ms5611 start adc start -# mag might be external +# Mag might be external if hmc5883 start then - echo "using HMC5883" + echo "[init] Using HMC5883" fi if mpu6000 start then - echo "using MPU6000" + echo "[init] Using MPU6000" fi if l3gd20 start then - echo "using L3GD20(H)" + echo "[init] Using L3GD20(H)" fi -if lsm303d start +if hw_ver compare PX4FMU_V2 then - echo "using LSM303D" + if lsm303d start + then + echo "[init] Using LSM303D" + fi fi # Start airspeed sensors if meas_airspeed start then - echo "using MEAS airspeed sensor" + echo "[init] Using MEAS airspeed sensor" else if ets_airspeed start then - echo "using ETS airspeed sensor (bus 3)" + echo "[init] Using ETS airspeed sensor (bus 3)" else if ets_airspeed start -b 1 then - echo "Using ETS airspeed sensor (bus 1)" + echo "[init] Using ETS airspeed sensor (bus 1)" fi fi fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 441d99ecf..92121ac17 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -14,6 +14,7 @@ set CONFIG_FILE /fs/microsd/etc/config.txt set EXTRAS_FILE /fs/microsd/etc/extras.txt set TUNE_OUT_ERROR ML<> $LOG_FILE tone_alarm MLL32CP8MB @@ -193,17 +193,17 @@ then usleep 500000 if px4io checkcrc $IO_FILE then - echo "[init] PX4IO CRC OK after updating" + echo "[init] PX4IO CRC OK, update successful" echo "PX4IO CRC OK after updating" >> $LOG_FILE tone_alarm MLL8CDE set IO_PRESENT yes else - echo "[init] PX4IO update failed" + echo "[init] ERROR: PX4IO update failed" echo "PX4IO update failed" >> $LOG_FILE fi else - echo "[init] PX4IO update failed" + echo "[init] ERROR: PX4IO update failed" echo "PX4IO update failed" >> $LOG_FILE fi fi @@ -220,16 +220,27 @@ then # if [ $OUTPUT_MODE == none ] then - if [ $IO_PRESENT == yes ] + if [ $USE_IO == yes ] then - # If PX4IO present, use it as primary PWM output by default set OUTPUT_MODE io else - # Else use PX4FMU PWM output set OUTPUT_MODE fmu fi fi + if [ $OUTPUT_MODE == io -a $IO_PRESENT != yes ] + then + # Need IO for output but it not present, disable output + set OUTPUT_MODE none + echo "[init] ERROR: PX4IO not found, disabling output" + + # Avoid using ttyS0 for MAVLink on FMUv1 + if hw_ver compare PX4FMU_V1 + then + set FMU_MODE serial + fi + fi + if [ $HIL == yes ] then set OUTPUT_MODE hil @@ -256,7 +267,7 @@ then echo "[init] PX4IO started" sh /etc/init.d/rc.io else - echo "[init] PX4IO start error" + echo "[init] ERROR: PX4IO start failed" tone_alarm $TUNE_OUT_ERROR fi fi @@ -267,7 +278,7 @@ then then echo "[init] FMU mode_$FMU_MODE started" else - echo "[init] FMU mode_$FMU_MODE start error" + echo "[init] ERROR: FMU mode_$FMU_MODE start failed" tone_alarm $TUNE_OUT_ERROR fi @@ -300,7 +311,7 @@ then then echo "[init] MKBLCTRL started" else - echo "[init] MKBLCTRL start error" + echo "[init] ERROR: MKBLCTRL start failed" tone_alarm $TUNE_OUT_ERROR fi @@ -312,7 +323,7 @@ then then echo "[init] HIL output started" else - echo "[init] HIL output error" + echo "[init] ERROR: HIL output start failed" tone_alarm $TUNE_OUT_ERROR fi fi @@ -329,7 +340,7 @@ then echo "[init] PX4IO started" sh /etc/init.d/rc.io else - echo "[init] PX4IO start error" + echo "[init] ERROR: PX4IO start failed" tone_alarm $TUNE_OUT_ERROR fi fi @@ -340,13 +351,17 @@ then then echo "[init] FMU mode_$FMU_MODE started" else - echo "[init] FMU mode_$FMU_MODE start error" + echo "[init] ERROR: FMU mode_$FMU_MODE start failed" tone_alarm $TUNE_OUT_ERROR fi if hw_ver compare PX4FMU_V1 then - if [ $FMU_MODE == pwm -o $FMU_MODE == gpio -o $FMU_MODE == pwm_gpio ] + if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ] + then + set TTYS1_BUSY yes + fi + if [ $FMU_MODE == pwm_gpio ] then set TTYS1_BUSY yes fi @@ -401,14 +416,22 @@ then then echo "[init] Vehicle type: FIXED WING" - if [ $FRAME_GEOMETRY == none ] + if [ $MIXER == none ] then - # Set default frame geometry for fixed wing - set FRAME_GEOMETRY AERT + # Set default mixer for fixed wing if not defined + set MIXER FMU_AERT fi + if [ $MAV_TYPE == none ] + then + # Use MAV_TYPE = 1 (fixed wing) if not defined + set MAV_TYPE 1 + fi + + param set MAV_TYPE $MAV_TYPE + # Load mixer and configure outputs - sh /etc/init.d/rc.fw_interface + sh /etc/init.d/rc.interface # Start standard fixedwing apps sh /etc/init.d/rc.fw_apps @@ -420,15 +443,37 @@ then if [ $VEHICLE_TYPE == mc ] then echo "[init] Vehicle type: MULTICOPTER" - - if [ $FRAME_GEOMETRY == none ] + + if [ $MIXER == none ] then - # Set default frame geometry for multicopter - set FRAME_GEOMETRY quad_x + # Set default mixer for multicopter if not defined + set MIXER quad_x + fi + + if [ $MAV_TYPE == none ] + then + # Use MAV_TYPE = 2 (quadcopter) if not defined + set MAV_TYPE 2 + + # Use mixer to detect vehicle type + if [ $MIXER == FMU_hex_x -o $MIXER == FMU_hex_+ ] + then + param set MAV_TYPE 13 + fi + if [ $MIXER == FMU_octo_x -o $MIXER == FMU_octo_+ ] + then + param set MAV_TYPE 14 + fi + if [ $MIXER == FMU_octo_cox ] + then + param set MAV_TYPE 14 + fi fi + + param set MAV_TYPE $MAV_TYPE # Load mixer and configure outputs - sh /etc/init.d/rc.mc_interface + sh /etc/init.d/rc.interface # Start standard multicopter apps sh /etc/init.d/rc.mc_apps @@ -440,9 +485,9 @@ then if [ $VEHICLE_TYPE == none ] then echo "[init] Vehicle type: GENERIC" - - attitude_estimator_ekf start - position_estimator_inav start + + # Load mixer and configure outputs + sh /etc/init.d/rc.interface fi # Start any custom addons -- cgit v1.2.3 From 778cbcb5cc28240aa1caeae4dd02c2a39567a0e1 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 15 Jan 2014 19:27:03 +1100 Subject: 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 41da63750..5525a8f33 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); } } -- cgit v1.2.3 From 84ad289e0ae8d2bd32cba5c22a27a35132b5c863 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 15 Jan 2014 17:20:08 +0100 Subject: 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 6a05ac3c6..beb9ad13d 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 83d09dd5e..96be1e8df 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 000000000..bcd24e5c9 --- /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 ff05cc322..223edc14a 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 0d3dabc00..77a4df618 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} }; -- cgit v1.2.3 From 352dea675435794d90f75d4a3c013d2afc439933 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 15 Jan 2014 20:04:11 +0100 Subject: 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 ba26ef28f..000000000 --- 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 4ba93fa74..000000000 --- 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 d918abd57..05bc7a5b3 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 8c6c300d6..3953b757d 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 -- cgit v1.2.3 From ff59aa9a0f856826682eb5099b1ec2525c5f7ba6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 16 Jan 2014 17:00:25 +1100 Subject: 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 5525a8f33..8bc18b3c2 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 -- cgit v1.2.3 From e5ad3c31e01680ccd18fe64d113ecf05de080e71 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 16 Jan 2014 12:46:20 +1100 Subject: 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 8bc18b3c2..a2a0c109c 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 -- cgit v1.2.3 From c304ea25077e1fd4675ef1d053cfc81e7e877b4b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 16 Jan 2014 08:37:50 +0100 Subject: Teached MTD test how to write back 0xFF after destructive test --- src/systemcmds/tests/test_mtd.c | 120 +++++----------------------------------- 1 file changed, 15 insertions(+), 105 deletions(-) diff --git a/src/systemcmds/tests/test_mtd.c b/src/systemcmds/tests/test_mtd.c index bcd24e5c9..8a626b65c 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; - // } - - // } + /* 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)); - // close(fd); + if (ret) { + warnx("ERROR! Could not fill file with 0xFF"); + close(fd); + return 1; + } } + (void)close(fd); + return 0; } -- cgit v1.2.3 From 8ffb9e29c62e645b48573439d4ebc70acfa7db54 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 16 Jan 2014 10:56:15 +0100 Subject: Teach IO firmware that arming and lockdown are two different things, clean up arming check --- src/modules/px4iofirmware/mixer.cpp | 15 +++++++++++++-- src/modules/px4iofirmware/protocol.h | 1 + src/modules/px4iofirmware/registers.c | 8 ++------ 3 files changed, 16 insertions(+), 8 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index e55ef784a..ea9e71c53 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -217,14 +217,25 @@ mixer_tick(void) } } - if ((should_arm || should_always_enable_pwm) && !mixer_servos_armed) { + /* set arming */ + bool needs_to_arm = (should_arm || should_always_enable_pwm); + + /* check any conditions that prevent arming */ + if (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) { + needs_to_arm = false; + } + if (!should_arm && !should_always_enable_pwm) { + needs_to_arm = false; + } + + if (needs_to_arm && !mixer_servos_armed) { /* need to arm, but not armed */ up_pwm_servo_arm(true); mixer_servos_armed = true; r_status_flags |= PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED; isr_debug(5, "> PWM enabled"); - } else if ((!should_arm && !should_always_enable_pwm) && mixer_servos_armed) { + } else if (!needs_to_arm && mixer_servos_armed) { /* armed but need to disarm */ up_pwm_servo_arm(false); mixer_servos_armed = false; diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index e5bef6eb3..ce8d26cc8 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -166,6 +166,7 @@ #define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK (1 << 4) /* OK to try in-air restart */ #define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE (1 << 5) /* Output of PWM right after startup enabled to help ESCs initialize and prevent them from beeping */ #define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED (1 << 6) /* Disable the IO-internal evaluation of the RC */ +#define PX4IO_P_SETUP_ARMING_LOCKDOWN (1 << 7) /* If set, the system operates normally, but won't actuate any servos */ #define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ #define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */ diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index ad4473073..42938b446 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -169,7 +169,8 @@ volatile uint16_t r_page_setup[] = PX4IO_P_SETUP_ARMING_IO_ARM_OK | \ PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \ PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \ - PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) + PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \ + PX4IO_P_SETUP_ARMING_LOCKDOWN) #define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1) #define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1) @@ -453,11 +454,6 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) * lockup of the IO arming state. */ - // XXX do not reset IO's safety state by FMU for now - // if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { - // r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED; - // } - if (value & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) { r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK; } -- cgit v1.2.3 From ded8cc6e14c7ec42d2a0e08b83c1510f213bf55d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 16 Jan 2014 10:56:39 +0100 Subject: Add IOCTLs to disable lockdown of an output port --- src/drivers/drv_pwm_output.h | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 51f916f37..53065f8eb 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -189,6 +189,20 @@ ORB_DECLARE(output_pwm); /** get the maximum PWM value the output will send */ #define PWM_SERVO_GET_MAX_PWM _IOC(_PWM_SERVO_BASE, 19) +/** set the lockdown override flag to enable outputs in HIL */ +#define PWM_SERVO_SET_DISABLE_LOCKDOWN _IOC(_PWM_SERVO_BASE, 21) + +/** get the lockdown override flag to enable outputs in HIL */ +#define PWM_SERVO_GET_DISABLE_LOCKDOWN _IOC(_PWM_SERVO_BASE, 22) + +/* + * + * + * WARNING WARNING WARNING! DO NOT EXCEED 31 IN IOC INDICES HERE! + * + * + */ + /** set a single servo to a specific value */ #define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo) -- cgit v1.2.3 From 978cfdccce9148ebb2282646b609d5eb7c59f487 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 16 Jan 2014 10:57:09 +0100 Subject: Teach IO driver how to disable lockdown mode --- src/drivers/px4io/px4io.cpp | 99 ++++++++++++++++++++++++++++++++++++++++----- 1 file changed, 89 insertions(+), 10 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 0ca35d2f2..658bcd8d8 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -273,6 +273,7 @@ private: actuator_outputs_s _outputs; ///ioctl(1, mode); /* XXX magic numbers */ - delete interface; + if (interface) { + result = interface->ioctl(1, mode); /* XXX magic numbers */ + delete interface; + } else { + errx(1, "interface not loaded, exiting"); + } errx(0, "test returned %d", result); } +void +lockdown(int argc, char *argv[]) +{ + if (g_dev != nullptr) { + + if (argc > 2 && !strcmp(argv[2], "disable")) { + + warnx("WARNING: ACTUATORS WILL BE LIVE IN HIL! PROCEED?"); + warnx("Press 'y' to enable, any other key to abort."); + + /* check if user wants to abort */ + char c; + + struct pollfd fds; + int ret; + hrt_abstime start = hrt_absolute_time(); + const unsigned long timeout = 5000000; + + while (hrt_elapsed_time(&start) < timeout) { + fds.fd = 0; /* stdin */ + fds.events = POLLIN; + ret = poll(&fds, 1, 0); + + if (ret > 0) { + + read(0, &c, 1); + + if (c != 'y') { + exit(0); + } else if (c == 'y') { + break; + } + } + + usleep(10000); + } + + if (hrt_elapsed_time(&start) > timeout) + errx(1, "TIMEOUT! ABORTED WITHOUT CHANGES."); + + (void)g_dev->ioctl(0, PWM_SERVO_SET_DISABLE_LOCKDOWN, 1); + + warnx("WARNING: ACTUATORS ARE NOW LIVE IN HIL!"); + } else { + (void)g_dev->ioctl(0, PWM_SERVO_SET_DISABLE_LOCKDOWN, 0); + warnx("ACTUATORS ARE NOW SAFE IN HIL."); + } + + } else { + errx(1, "driver not loaded, exiting"); + } + exit(0); +} + } /* namespace */ int @@ -2883,6 +2959,9 @@ px4io_main(int argc, char *argv[]) if (!strcmp(argv[1], "bind")) bind(argc, argv); + if (!strcmp(argv[1], "lockdown")) + lockdown(argc, argv); + out: errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'bind', 'checkcrc', 'forceupdate' or 'update'"); } -- cgit v1.2.3 From 62076c0e81d505544ecddb05b039a4f010200480 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 16 Jan 2014 10:58:05 +0100 Subject: Teach the commander to arm on the commandline --- src/modules/commander/commander.cpp | 57 ++++++++++++++++++++++++++++++------- 1 file changed, 47 insertions(+), 10 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index add7312de..2a2bcca72 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -154,6 +154,16 @@ static bool on_usb_power = false; static float takeoff_alt = 5.0f; +static struct vehicle_status_s status; + +/* armed topic */ +static struct actuator_armed_s armed; + +static struct safety_s safety; + +/* flags for control apps */ +struct vehicle_control_mode_s control_mode; + /* tasks waiting for low prio thread */ typedef enum { LOW_PRIO_TASK_NONE = 0, @@ -210,6 +220,9 @@ void print_reject_arm(const char *msg); void print_status(); +int arm(); +int disarm(); + transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos); /** @@ -277,6 +290,16 @@ int commander_main(int argc, char *argv[]) exit(0); } + if (!strcmp(argv[1], "arm")) { + arm(); + exit(0); + } + + if (!strcmp(argv[1], "disarm")) { + disarm(); + exit(0); + } + usage("unrecognized command"); exit(1); } @@ -344,6 +367,30 @@ void print_status() static orb_advert_t control_mode_pub; static orb_advert_t status_pub; +int arm() +{ + int arming_res = arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED, &armed); + + if (arming_res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline"); + return 0; + } else { + return 1; + } +} + +int disarm() +{ + int arming_res = arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed); + + if (arming_res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline"); + return 0; + } else { + return 1; + } +} + void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed) { /* result of the command */ @@ -537,16 +584,6 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } -static struct vehicle_status_s status; - -/* armed topic */ -static struct actuator_armed_s armed; - -static struct safety_s safety; - -/* flags for control apps */ -struct vehicle_control_mode_s control_mode; - int commander_thread_main(int argc, char *argv[]) { /* not yet initialized */ -- cgit v1.2.3 From a0bb6674da5c0ca9c23f1db91bc9506c75242398 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 16 Jan 2014 19:03:14 +0100 Subject: 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 264d911f3..7cfca7656 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) -- cgit v1.2.3 From 2600c96e92de4ce8123b20210176456ec7e5332d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 16 Jan 2014 20:13:17 +0100 Subject: 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 c3f39addf..1be4877ba 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 *************************************************************/ -- cgit v1.2.3 From 71b11d54e0cc441dabed878db270881a6308a7c7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 16 Jan 2014 20:13:35 +0100 Subject: 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 8da555211..8152ea4d4 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 *******************************************************/ -- cgit v1.2.3 From e691bab71a69216273fdc253695ef849671af9a7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 16 Jan 2014 22:46:55 +0100 Subject: 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 8a626b65c..bac9efbdb 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; } -- cgit v1.2.3 From 0f30db08c0d44e753005b2a40fef8900ed5dba33 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 17 Jan 2014 15:44:03 +0100 Subject: Small documentation correction --- src/systemcmds/tests/tests.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h index 223edc14a..82de05dff 100644 --- a/src/systemcmds/tests/tests.h +++ b/src/systemcmds/tests/tests.h @@ -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 -- cgit v1.2.3 From c94eb3198aa37e57643fb3373d14ecef46807935 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 17 Jan 2014 23:15:23 +0100 Subject: rcS: use mtd on FMUv1 too --- ROMFS/px4fmu_common/init.d/rcS | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 92121ac17..32c7991ef 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -75,12 +75,9 @@ then # Load parameters # set PARAM_FILE /fs/microsd/params - if hw_ver compare PX4FMU_V2 + if mtd start then - if mtd start - then - set PARAM_FILE /fs/mtd_params - fi + set PARAM_FILE /fs/mtd_params fi param select $PARAM_FILE -- cgit v1.2.3 From 5db68264c7b1240811d28d04149e4a49891ab423 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 18 Jan 2014 00:07:25 +0100 Subject: rcS: HIL fixed --- ROMFS/px4fmu_common/init.d/rcS | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 32c7991ef..6325bb94f 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -241,6 +241,7 @@ then if [ $HIL == yes ] then set OUTPUT_MODE hil + set FMU_MODE serial else # Try to get an USB console if not in HIL mode nshterm /dev/ttyACM0 & @@ -373,6 +374,7 @@ then if [ $HIL == yes ] then + sleep 1 mavlink start -b 230400 -d /dev/ttyACM0 usleep 5000 else -- cgit v1.2.3 From 4d7e99fd6c47cb94d63a118d5557eefbe6df8f2e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 18 Jan 2014 01:09:58 +0100 Subject: Writing RSSI field not only in servo rail topic --- src/drivers/px4io/px4io.cpp | 20 +++++++++++++------- 1 file changed, 13 insertions(+), 7 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 0ca35d2f2..7cc7d3b6d 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -270,7 +270,8 @@ private: orb_advert_t _to_servorail; ///< servorail status orb_advert_t _to_safety; ///< status of safety - actuator_outputs_s _outputs; /// 0) { - orb_publish(ORB_ID(servorail_status), _to_servorail, &servorail_status); + orb_publish(ORB_ID(servorail_status), _to_servorail, &_servorail_status); } else { - _to_servorail = orb_advertise(ORB_ID(servorail_status), &servorail_status); + _to_servorail = orb_advertise(ORB_ID(servorail_status), &_servorail_status); } } @@ -1450,6 +1451,11 @@ PX4IO::io_publish_raw_rc() rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN; } + /* set RSSI */ + + // XXX the correct scaling needs to be validated here + rc_val.rssi = _servorail_status.rssi_v / 3.3f; + /* lazily advertise on first publication */ if (_to_input_rc == 0) { _to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_val); -- cgit v1.2.3 From dd9df7b1b0974a9838d3e21842a0d90f3eff54d9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 18 Jan 2014 01:11:36 +0100 Subject: RSSI field init --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 7cc7d3b6d..dede5976d 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1454,7 +1454,7 @@ PX4IO::io_publish_raw_rc() /* set RSSI */ // XXX the correct scaling needs to be validated here - rc_val.rssi = _servorail_status.rssi_v / 3.3f; + rc_val.rssi = (_servorail_status.rssi_v / 3.3f) * UINT8_MAX; /* lazily advertise on first publication */ if (_to_input_rc == 0) { -- cgit v1.2.3 From d174998b45349348ffe41150aa1d22d7d943b790 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 18 Jan 2014 12:00:17 +0100 Subject: RSSI and concurrent S.Bus output handling --- src/drivers/px4io/px4io.cpp | 6 ++++-- src/modules/px4iofirmware/controls.c | 9 +++++++++ src/modules/px4iofirmware/px4io.c | 5 +++++ src/modules/px4iofirmware/px4io.h | 1 + 4 files changed, 19 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index dede5976d..8e080c043 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1453,8 +1453,10 @@ PX4IO::io_publish_raw_rc() /* set RSSI */ - // XXX the correct scaling needs to be validated here - rc_val.rssi = (_servorail_status.rssi_v / 3.3f) * UINT8_MAX; + if (rc_val.input_source != RC_INPUT_SOURCE_PX4IO_SBUS) { + // XXX the correct scaling needs to be validated here + rc_val.rssi = (_servorail_status.rssi_v / 3.3f) * UINT8_MAX; + } /* lazily advertise on first publication */ if (_to_input_rc == 0) { diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 541eed0e1..5859f768b 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -114,9 +114,18 @@ controls_tick() { perf_begin(c_gather_sbus); bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &rssi, PX4IO_RC_INPUT_CHANNELS); + + bool sbus_status = (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS); + if (sbus_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS; } + + /* switch S.Bus output pin as needed */ + if (sbus_status != (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS)) { + ENABLE_SBUS_OUT((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS)); + } + perf_end(c_gather_sbus); /* diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index 0b8c4a6a8..150af35df 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -196,6 +196,11 @@ user_start(int argc, char *argv[]) POWER_SERVO(true); #endif + /* turn off S.Bus out (if supported) */ +#ifdef ENABLE_SBUS_OUT + ENABLE_SBUS_OUT(false); +#endif + /* start the safety switch handler */ safety_init(); diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index a0daa97ea..18c7468f6 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -160,6 +160,7 @@ extern pwm_limit_t pwm_limit; # define PX4IO_RELAY_CHANNELS 0 # define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s)) +# define ENABLE_SBUS_OUT(_s) stm32_gpiowrite(GPIO_SBUS_OENABLE, (_s)) # define VDD_SERVO_FAULT (!stm32_gpioread(GPIO_SERVO_FAULT_DETECT)) -- cgit v1.2.3 From bb8cf0894f01e9889a261d4cd62798048c0cf7f5 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 18 Jan 2014 16:47:23 +0100 Subject: autostart: HIL and bad PX4IO fixes --- ROMFS/px4fmu_common/init.d/rcS | 165 +++++++++++++++++++++-------------------- 1 file changed, 86 insertions(+), 79 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 6325bb94f..a9c5c59ea 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -241,7 +241,10 @@ then if [ $HIL == yes ] then set OUTPUT_MODE hil - set FMU_MODE serial + if hw_ver compare PX4FMU_V1 + then + set FMU_MODE serial + fi else # Try to get an USB console if not in HIL mode nshterm /dev/ttyACM0 & @@ -257,82 +260,12 @@ then # set TTYS1_BUSY no - if [ $OUTPUT_MODE == io ] + # If OUTPUT_MODE == none then something is wrong with setup and we shouldn't try to enable output + if [ $OUTPUT_MODE != none ] then - echo "[init] Use PX4IO PWM as primary output" - if px4io start - then - echo "[init] PX4IO started" - sh /etc/init.d/rc.io - else - echo "[init] ERROR: PX4IO start failed" - tone_alarm $TUNE_OUT_ERROR - fi - fi - if [ $OUTPUT_MODE == fmu ] - then - echo "[init] Use FMU PWM as primary output" - if fmu mode_$FMU_MODE - then - echo "[init] FMU mode_$FMU_MODE started" - else - echo "[init] ERROR: FMU mode_$FMU_MODE start failed" - tone_alarm $TUNE_OUT_ERROR - fi - - if hw_ver compare PX4FMU_V1 - then - if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ] - then - set TTYS1_BUSY yes - fi - if [ $FMU_MODE == pwm_gpio ] - then - set TTYS1_BUSY yes - fi - fi - fi - if [ $OUTPUT_MODE == mkblctrl ] - then - echo "[init] Use MKBLCTRL as primary output" - set MKBLCTRL_ARG "" - if [ $MKBLCTRL_MODE == x ] - then - set MKBLCTRL_ARG "-mkmode x" - fi - if [ $MKBLCTRL_MODE == + ] - then - set MKBLCTRL_ARG "-mkmode +" - fi - - if mkblctrl $MKBLCTRL_ARG - then - echo "[init] MKBLCTRL started" - else - echo "[init] ERROR: MKBLCTRL start failed" - tone_alarm $TUNE_OUT_ERROR - fi - - fi - if [ $OUTPUT_MODE == hil ] - then - echo "[init] Use HIL as primary output" - if hil mode_pwm - then - echo "[init] HIL output started" - else - echo "[init] ERROR: HIL output start failed" - tone_alarm $TUNE_OUT_ERROR - fi - fi - - # - # Start IO or FMU for RC PPM input if needed - # - if [ $IO_PRESENT == yes ] - then - if [ $OUTPUT_MODE != io ] + if [ $OUTPUT_MODE == io ] then + echo "[init] Use PX4IO PWM as primary output" if px4io start then echo "[init] PX4IO started" @@ -342,9 +275,9 @@ then tone_alarm $TUNE_OUT_ERROR fi fi - else - if [ $OUTPUT_MODE != fmu ] + if [ $OUTPUT_MODE == fmu ] then + echo "[init] Use FMU PWM as primary output" if fmu mode_$FMU_MODE then echo "[init] FMU mode_$FMU_MODE started" @@ -352,7 +285,7 @@ then echo "[init] ERROR: FMU mode_$FMU_MODE start failed" tone_alarm $TUNE_OUT_ERROR fi - + if hw_ver compare PX4FMU_V1 then if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ] @@ -365,8 +298,82 @@ then fi fi fi + if [ $OUTPUT_MODE == mkblctrl ] + then + echo "[init] Use MKBLCTRL as primary output" + set MKBLCTRL_ARG "" + if [ $MKBLCTRL_MODE == x ] + then + set MKBLCTRL_ARG "-mkmode x" + fi + if [ $MKBLCTRL_MODE == + ] + then + set MKBLCTRL_ARG "-mkmode +" + fi + + if mkblctrl $MKBLCTRL_ARG + then + echo "[init] MKBLCTRL started" + else + echo "[init] ERROR: MKBLCTRL start failed" + tone_alarm $TUNE_OUT_ERROR + fi + + fi + if [ $OUTPUT_MODE == hil ] + then + echo "[init] Use HIL as primary output" + if hil mode_pwm + then + echo "[init] HIL output started" + else + echo "[init] ERROR: HIL output start failed" + tone_alarm $TUNE_OUT_ERROR + fi + fi + + # + # Start IO or FMU for RC PPM input if needed + # + if [ $IO_PRESENT == yes ] + then + if [ $OUTPUT_MODE != io ] + then + if px4io start + then + echo "[init] PX4IO started" + sh /etc/init.d/rc.io + else + echo "[init] ERROR: PX4IO start failed" + tone_alarm $TUNE_OUT_ERROR + fi + fi + else + if [ $OUTPUT_MODE != fmu ] + then + if fmu mode_$FMU_MODE + then + echo "[init] FMU mode_$FMU_MODE started" + else + echo "[init] ERROR: FMU mode_$FMU_MODE start failed" + tone_alarm $TUNE_OUT_ERROR + fi + + if hw_ver compare PX4FMU_V1 + then + if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ] + then + set TTYS1_BUSY yes + fi + if [ $FMU_MODE == pwm_gpio ] + then + set TTYS1_BUSY yes + fi + fi + fi + fi fi - + # # MAVLink # -- cgit v1.2.3 From a9e288dfdd803cf43136a1c413f5fd86d9bd2794 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 18 Jan 2014 20:08:08 +0100 Subject: rc: bug fixed --- ROMFS/px4fmu_common/init.d/rc.interface | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index 928d3aeeb..d25f01dde 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -33,7 +33,7 @@ then tone_alarm $TUNE_OUT_ERROR fi else - echo "[init] Mixer not defined + echo "[init] Mixer not defined" tone_alarm $TUNE_OUT_ERROR fi -- cgit v1.2.3 From d96e63960c9f951dac02f122ac1e067f7dd3316f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 19 Jan 2014 12:42:43 +0100 Subject: enable new autostart scripts for Rascal HIL and X5 --- .../px4fmu_common/init.d/1004_rc_fw_Rascal110.hil | 68 +++------------------- ROMFS/px4fmu_common/init.d/rc.autostart | 4 +- 2 files changed, 10 insertions(+), 62 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil index 344d78422..75a00a675 100644 --- a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil +++ b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil @@ -1,14 +1,13 @@ #!nsh # -# USB HIL start +# HIL Rascal 110 (Flightgear) +# +# Maintainers: Thomas Gubler # -echo "[HIL] HILStar starting.." +echo "HIL Rascal 110 starting.." -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then # Set all params here, then disable autoconfig @@ -40,59 +39,8 @@ then param save fi -# Allow USB some time to come up -sleep 1 -# Tell MAVLink that this link is "fast" -mavlink start -b 230400 -d /dev/ttyACM0 - -# Create a fake HIL /dev/pwm_output interface -hil mode_pwm - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ -# -param set MAV_TYPE 1 - -# -# Check if we got an IO -# -if px4io start -then - echo "IO started" -else - fmu mode_serial - echo "FMU started" -fi - -# -# Start the sensors (depends on orb, px4io) -# -sh /etc/init.d/rc.sensors - -# -# Start the attitude estimator (depends on orb) -# -att_pos_estimator_ekf start - -# -# Load mixer and start controllers (depends on px4io) -# -set MODE autostart -mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix -if [ -f /fs/microsd/etc/mixers/FMU_AERT.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_AERT.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_AERT.mix -else - echo "Using /etc/mixers/FMU_AERT.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix -fi - - -fw_pos_control_l1 start -fw_att_control start +set HIL yes -echo "[HIL] setup done, running" +set VEHICLE_TYPE fw +set MIXER FMU_AERT diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 69e88fcd0..325520dd0 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -43,7 +43,7 @@ fi if param compare SYS_AUTOSTART 1004 then - #sh /etc/init.d/1004_rc_fw_Rascal110.hil + sh /etc/init.d/1004_rc_fw_Rascal110.hil fi # @@ -84,7 +84,7 @@ fi if param compare SYS_AUTOSTART 3032 then - #sh /etc/init.d/3032_skywalker_x5 + sh /etc/init.d/3032_skywalker_x5 fi if param compare SYS_AUTOSTART 3033 -- cgit v1.2.3 From 40a0ac5736c35eeecb7e73050e5c685bf3195361 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 19 Jan 2014 20:59:15 +0100 Subject: sdlog2: use GPS time for naming log dirs and files, some fixes --- src/modules/sdlog2/sdlog2.c | 238 +++++++++++++++++++++++++++----------------- 1 file changed, 148 insertions(+), 90 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 06b1eddaa..e9d809834 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -108,7 +108,7 @@ static bool main_thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int deamon_task; /**< Handle of deamon task / thread */ static bool logwriter_should_exit = false; /**< Logwriter thread exit flag */ -static const int MAX_NO_LOGFOLDER = 999; /**< Maximum number of log folders */ +static const int MAX_NO_LOGFOLDER = 999; /**< Maximum number of log dirs */ static const int MAX_NO_LOGFILE = 999; /**< Maximum number of log files */ static const int LOG_BUFFER_SIZE_DEFAULT = 8192; static const int MAX_WRITE_CHUNK = 512; @@ -122,14 +122,17 @@ struct logbuffer_s lb; static pthread_mutex_t logbuffer_mutex; static pthread_cond_t logbuffer_cond; -static char folder_path[64]; +static char log_dir[32]; /* statistics counters */ -static unsigned long log_bytes_written = 0; static uint64_t start_time = 0; +static unsigned long log_bytes_written = 0; static unsigned long log_msgs_written = 0; static unsigned long log_msgs_skipped = 0; +/* GPS time, used for log files naming */ +static uint64_t gps_time = 0; + /* current state of logging */ static bool logging_enabled = false; /* enable logging on start (-e option) */ @@ -138,11 +141,14 @@ static bool log_on_start = false; static bool log_when_armed = false; /* delay = 1 / rate (rate defined by -r option) */ static useconds_t sleep_delay = 0; +/* use date/time for naming directories and files (-t option) */ +static bool log_name_timestamp = false; /* helper flag to track system state changes */ static bool flag_system_armed = false; static pthread_t logwriter_pthread = 0; +static pthread_attr_t logwriter_attr; /** * Log buffer writing thread. Open and close file here. @@ -203,9 +209,9 @@ static void handle_command(struct vehicle_command_s *cmd); static void handle_status(struct vehicle_status_s *cmd); /** - * Create folder for current logging session. Store folder name in 'log_folder'. + * Create dir for current logging session. Store dir name in 'log_dir'. */ -static int create_logfolder(void); +static int create_logdir(void); /** * Select first free log file name and open it. @@ -218,11 +224,12 @@ sdlog2_usage(const char *reason) if (reason) fprintf(stderr, "%s\n", reason); - errx(1, "usage: sdlog2 {start|stop|status} [-r ] [-b ] -e -a\n" + errx(1, "usage: sdlog2 {start|stop|status} [-r ] [-b ] -e -a -t\n" "\t-r\tLog rate in Hz, 0 means unlimited rate\n" "\t-b\tLog buffer size in KiB, default is 8\n" "\t-e\tEnable logging by default (if not, can be started by command)\n" - "\t-a\tLog only when armed (can be still overriden by command)\n"); + "\t-a\tLog only when armed (can be still overriden by command)\n" + "\t-t\tUse date/time for naming log directories and files\n"); } /** @@ -280,82 +287,101 @@ int sdlog2_main(int argc, char *argv[]) exit(1); } -int create_logfolder() +int create_log_dir() { - /* make folder on sdcard */ - uint16_t folder_number = 1; // start with folder sess001 + /* create dir on sdcard if needed */ + uint16_t dir_number = 1; // start with dir sess001 int mkdir_ret; - /* look for the next folder that does not exist */ - while (folder_number <= MAX_NO_LOGFOLDER) { - /* set up folder path: e.g. /fs/microsd/sess001 */ - sprintf(folder_path, "%s/sess%03u", mountpoint, folder_number); - mkdir_ret = mkdir(folder_path, S_IRWXU | S_IRWXG | S_IRWXO); - /* the result is -1 if the folder exists */ - - if (mkdir_ret == 0) { - /* folder does not exist, success */ + if (log_name_timestamp && gps_time != 0) { + /* use GPS date for log dir naming: e.g. /fs/microsd/2014-01-19 */ + time_t gps_time_sec = gps_time / 1000000; + struct tm t; + gmtime_r(&gps_time_sec, &t); + int n = snprintf(log_dir, sizeof(log_dir), "%s/", mountpoint); + strftime(log_dir + n, sizeof(log_dir) - n, "%Y-%m-%d", &t); + mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO); + if (mkdir_ret != 0 && errno != EEXIST) { + warn("failed creating new dir"); + return -1; + } + } else { + /* look for the next dir that does not exist */ + while (dir_number <= MAX_NO_LOGFOLDER) { + /* format log dir: e.g. /fs/microsd/sess001 */ + sprintf(log_dir, "%s/sess%03u", mountpoint, dir_number); + mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO); + + if (mkdir_ret != 0) { + if (errno == EEXIST) { + /* dir exists already */ + dir_number++; + continue; + } else { + warn("failed creating new dir"); + return -1; + } + } + /* dir does not exist, success */ break; + } - } else if (mkdir_ret == -1) { - /* folder exists already */ - folder_number++; - continue; - - } else { - warn("failed creating new folder"); + if (dir_number >= MAX_NO_LOGFOLDER) { + /* we should not end up here, either we have more than MAX_NO_LOGFOLDER on the SD card, or another problem */ + warnx("all %d possible dirs exist already.", MAX_NO_LOGFOLDER); return -1; } } - if (folder_number >= MAX_NO_LOGFOLDER) { - /* we should not end up here, either we have more than MAX_NO_LOGFOLDER on the SD card, or another problem */ - warnx("all %d possible folders exist already.", MAX_NO_LOGFOLDER); - return -1; - } - + /* print logging path, important to find log file later */ + warnx("logging to dir: %s", log_dir); + mavlink_log_info(mavlink_fd, "[sdlog2] logging to dir: %s", log_dir); return 0; } int open_logfile() { - /* make folder on sdcard */ - uint16_t file_number = 1; // start with file log001 - /* string to hold the path to the log */ - char path_buf[64] = ""; - - int fd = 0; - - /* look for the next file that does not exist */ - while (file_number <= MAX_NO_LOGFILE) { - /* set up file path: e.g. /fs/microsd/sess001/log001.bin */ - sprintf(path_buf, "%s/log%03u.bin", folder_path, file_number); - - if (file_exist(path_buf)) { + char log_file[64] = ""; + + if (log_name_timestamp && gps_time != 0) { + /* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ + int n = snprintf(log_file, sizeof(log_file), "%s/", log_dir); + time_t gps_time_sec = gps_time / 1000000; + struct tm t; + gmtime_r(&gps_time_sec, &t); + n += strftime(log_file + n, sizeof(log_file) - n, "%H_%M_%S.bin", &t); + } else { + uint16_t file_number = 1; // start with file log001 + /* look for the next file that does not exist */ + while (file_number <= MAX_NO_LOGFILE) { + /* format log file path: e.g. /fs/microsd/sess001/log001.bin */ + snprintf(log_file, sizeof(log_file), "%s/log%03u.bin", log_dir, file_number); + + if (!file_exist(log_file)) { + break; + } file_number++; - continue; } - fd = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC); - - if (fd == 0) { - warn("opening %s failed", path_buf); + if (file_number > MAX_NO_LOGFILE) { + /* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */ + warnx("all %d possible files exist already.", MAX_NO_LOGFILE); + return -1; } - - warnx("logging to: %s.", path_buf); - mavlink_log_info(mavlink_fd, "[sdlog2] log: %s", path_buf); - - return fd; } - if (file_number > MAX_NO_LOGFILE) { - /* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */ - warnx("all %d possible files exist already.", MAX_NO_LOGFILE); - return -1; + int fd = open(log_file, O_CREAT | O_WRONLY | O_DSYNC); + + if (fd < 0) { + warn("failed opening %s", log_file); + mavlink_log_info(mavlink_fd, "[sdlog2] failed opening %s", log_file); + } else { + warnx("logging to: %s", log_file); + mavlink_log_info(mavlink_fd, "[sdlog2] log: %s", log_file); } - return 0; + return fd; } static void *logwriter_thread(void *arg) @@ -363,10 +389,13 @@ static void *logwriter_thread(void *arg) /* set name */ prctl(PR_SET_NAME, "sdlog2_writer", 0); - struct logbuffer_s *logbuf = (struct logbuffer_s *)arg; - int log_fd = open_logfile(); + if (log_fd < 0) + return; + + struct logbuffer_s *logbuf = (struct logbuffer_s *)arg; + /* write log messages formats, version and parameters */ log_bytes_written += write_formats(log_fd); log_bytes_written += write_version(log_fd); @@ -443,7 +472,7 @@ static void *logwriter_thread(void *arg) fsync(log_fd); close(log_fd); - return OK; + return; } void sdlog2_start_log() @@ -451,6 +480,22 @@ void sdlog2_start_log() warnx("start logging."); mavlink_log_info(mavlink_fd, "[sdlog2] start logging"); + /* create log dir if needed */ + if (create_log_dir() != 0) { + mavlink_log_critical(mavlink_fd, "[sdlog2] error creating log dir"); + errx(1, "error creating log dir"); + } + + const char *converter_in = "/etc/logging/conv.zip"; + char *converter_out = malloc(120); + sprintf(converter_out, "%s/conv.zip", log_dir); + + if (file_copy(converter_in, converter_out)) { + warnx("unable to copy conversion scripts"); + } + + free(converter_out); + /* initialize statistics counter */ log_bytes_written = 0; start_time = hrt_absolute_time(); @@ -458,25 +503,23 @@ void sdlog2_start_log() log_msgs_skipped = 0; /* initialize log buffer emptying thread */ - pthread_attr_t receiveloop_attr; - pthread_attr_init(&receiveloop_attr); + pthread_attr_init(&logwriter_attr); struct sched_param param; /* low priority, as this is expensive disk I/O */ param.sched_priority = SCHED_PRIORITY_DEFAULT - 40; - (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); + (void)pthread_attr_setschedparam(&logwriter_attr, ¶m); - pthread_attr_setstacksize(&receiveloop_attr, 2048); + pthread_attr_setstacksize(&logwriter_attr, 2048); logwriter_should_exit = false; /* start log buffer emptying thread */ - if (0 != pthread_create(&logwriter_pthread, &receiveloop_attr, logwriter_thread, &lb)) { + if (0 != pthread_create(&logwriter_pthread, &logwriter_attr, logwriter_thread, &lb)) { errx(1, "error creating logwriter thread"); } logging_enabled = true; - // XXX we have to destroy the attr at some point } void sdlog2_stop_log() @@ -501,6 +544,7 @@ void sdlog2_stop_log() } logwriter_pthread = 0; + pthread_attr_destroy(&logwriter_attr); sdlog2_status(); } @@ -594,12 +638,19 @@ int sdlog2_thread_main(int argc, char *argv[]) /* log buffer size */ int log_buffer_size = LOG_BUFFER_SIZE_DEFAULT; + logging_enabled = false; + log_on_start = false; + log_when_armed = false; + log_name_timestamp = false; + + flag_system_armed = false; + /* work around some stupidity in task_create's argv handling */ argc -= 2; argv += 2; int ch; - while ((ch = getopt(argc, argv, "r:b:ea")) != EOF) { + while ((ch = getopt(argc, argv, "r:b:eat")) != EOF) { switch (ch) { case 'r': { unsigned long r = strtoul(optarg, NULL, 10); @@ -632,6 +683,10 @@ int sdlog2_thread_main(int argc, char *argv[]) log_when_armed = true; break; + case 't': + log_name_timestamp = true; + break; + case '?': if (optopt == 'c') { warnx("Option -%c requires an argument.", optopt); @@ -649,27 +704,12 @@ int sdlog2_thread_main(int argc, char *argv[]) } } + gps_time = 0; + if (!file_exist(mountpoint)) { errx(1, "logging mount point %s not present, exiting.", mountpoint); } - if (create_logfolder()) { - errx(1, "unable to create logging folder, exiting."); - } - - const char *converter_in = "/etc/logging/conv.zip"; - char *converter_out = malloc(120); - sprintf(converter_out, "%s/conv.zip", folder_path); - - if (file_copy(converter_in, converter_out)) { - errx(1, "unable to copy conversion scripts, exiting."); - } - - free(converter_out); - - /* only print logging path, important to find log file later */ - warnx("logging to directory: %s", folder_path); - /* initialize log buffer with specified size */ warnx("log buffer size: %i bytes.", log_buffer_size); @@ -919,15 +959,22 @@ int sdlog2_thread_main(int argc, char *argv[]) uint16_t differential_pressure_counter = 0; /* enable logging on start if needed */ - if (log_on_start) + if (log_on_start) { + /* check GPS topic to get GPS time */ + if (log_name_timestamp) { + if (OK == orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos)) { + gps_time = buf.gps_pos.time_gps_usec; + } + } sdlog2_start_log(); + } while (!main_thread_should_exit) { /* decide use usleep() or blocking poll() */ bool use_sleep = sleep_delay > 0 && logging_enabled; /* poll all topics if logging enabled or only management (first 2) if not */ - int poll_ret = poll(fds, logging_enabled ? fdsc_count : 2, use_sleep ? 0 : poll_timeout); + int poll_ret = poll(fds, logging_enabled ? fdsc_count : 3, use_sleep ? 0 : poll_timeout); /* handle the poll result */ if (poll_ret < 0) { @@ -960,6 +1007,17 @@ int sdlog2_thread_main(int argc, char *argv[]) handled_topics++; } + /* --- GPS POSITION - LOG MANAGEMENT --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); + + if (log_name_timestamp) { + gps_time = buf.gps_pos.time_gps_usec; + } + + handled_topics++; + } + if (!logging_enabled || !check_data || handled_topics >= poll_ret) { continue; } @@ -988,7 +1046,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- GPS POSITION --- */ if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); + // Don't orb_copy, it's already done few lines above log_msg.msg_type = LOG_GPS_MSG; log_msg.body.log_GPS.gps_time = buf.gps_pos.time_gps_usec; log_msg.body.log_GPS.fix_type = buf.gps_pos.fix_type; -- cgit v1.2.3 From 5e3c365cd4809def0c5b21136a1b5741a98ae35e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 19 Jan 2014 22:56:24 +0100 Subject: rc: use sdlog2 -t option sdlog2: move all logs and conv.zip to "log" dir, messages cleanup --- ROMFS/px4fmu_common/init.d/rc.logging | 4 +- src/modules/sdlog2/sdlog2.c | 129 +++++++++++++++++----------------- 2 files changed, 68 insertions(+), 65 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging index dc4be8055..6c02bf227 100644 --- a/ROMFS/px4fmu_common/init.d/rc.logging +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -7,8 +7,8 @@ if [ -d /fs/microsd ] then if [ $BOARD == fmuv1 ] then - sdlog2 start -r 50 -a -b 16 + sdlog2 start -r 50 -a -b 16 -t else - sdlog2 start -r 200 -a -b 16 + sdlog2 start -r 200 -a -b 16 -t fi fi diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index e9d809834..656575573 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -114,7 +114,7 @@ static const int LOG_BUFFER_SIZE_DEFAULT = 8192; static const int MAX_WRITE_CHUNK = 512; static const int MIN_BYTES_TO_WRITE = 512; -static const char *mountpoint = "/fs/microsd"; +static const char *log_root = "/fs/microsd/log"; static int mavlink_fd = -1; struct logbuffer_s lb; @@ -211,12 +211,12 @@ static void handle_status(struct vehicle_status_s *cmd); /** * Create dir for current logging session. Store dir name in 'log_dir'. */ -static int create_logdir(void); +static int create_log_dir(void); /** * Select first free log file name and open it. */ -static int open_logfile(void); +static int open_log_file(void); static void sdlog2_usage(const char *reason) @@ -298,67 +298,69 @@ int create_log_dir() time_t gps_time_sec = gps_time / 1000000; struct tm t; gmtime_r(&gps_time_sec, &t); - int n = snprintf(log_dir, sizeof(log_dir), "%s/", mountpoint); + int n = snprintf(log_dir, sizeof(log_dir), "%s/", log_root); strftime(log_dir + n, sizeof(log_dir) - n, "%Y-%m-%d", &t); mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO); - if (mkdir_ret != 0 && errno != EEXIST) { - warn("failed creating new dir"); + if (mkdir_ret == OK) { + warnx("log dir created: %s", log_dir); + } else if (errno != EEXIST) { + warn("failed creating new dir: %s", log_dir); return -1; } } else { /* look for the next dir that does not exist */ while (dir_number <= MAX_NO_LOGFOLDER) { /* format log dir: e.g. /fs/microsd/sess001 */ - sprintf(log_dir, "%s/sess%03u", mountpoint, dir_number); + sprintf(log_dir, "%s/sess%03u", log_root, dir_number); mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO); - if (mkdir_ret != 0) { - if (errno == EEXIST) { - /* dir exists already */ - dir_number++; - continue; - } else { - warn("failed creating new dir"); - return -1; - } + if (mkdir_ret == 0) { + warnx("log dir created: %s", log_dir); + break; + } else if (errno != EEXIST) { + warn("failed creating new dir: %s", log_dir); + return -1; } - /* dir does not exist, success */ - break; + /* dir exists already */ + dir_number++; + continue; } if (dir_number >= MAX_NO_LOGFOLDER) { /* we should not end up here, either we have more than MAX_NO_LOGFOLDER on the SD card, or another problem */ - warnx("all %d possible dirs exist already.", MAX_NO_LOGFOLDER); + warnx("all %d possible dirs exist already", MAX_NO_LOGFOLDER); return -1; } } /* print logging path, important to find log file later */ - warnx("logging to dir: %s", log_dir); - mavlink_log_info(mavlink_fd, "[sdlog2] logging to dir: %s", log_dir); + warnx("log dir: %s", log_dir); + mavlink_log_info(mavlink_fd, "[sdlog2] log dir: %s", log_dir); return 0; } -int open_logfile() +int open_log_file() { /* string to hold the path to the log */ - char log_file[64] = ""; + char log_file_name[16] = ""; + char log_file_path[48] = ""; if (log_name_timestamp && gps_time != 0) { /* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ - int n = snprintf(log_file, sizeof(log_file), "%s/", log_dir); time_t gps_time_sec = gps_time / 1000000; struct tm t; gmtime_r(&gps_time_sec, &t); - n += strftime(log_file + n, sizeof(log_file) - n, "%H_%M_%S.bin", &t); + strftime(log_file_name, sizeof(log_file_name), "%H_%M_%S.bin", &t); + snprintf(log_file_path, sizeof(log_file_path), "%s/%s", log_dir, log_file_name); } else { uint16_t file_number = 1; // start with file log001 /* look for the next file that does not exist */ while (file_number <= MAX_NO_LOGFILE) { /* format log file path: e.g. /fs/microsd/sess001/log001.bin */ - snprintf(log_file, sizeof(log_file), "%s/log%03u.bin", log_dir, file_number); + snprintf(log_file_name, sizeof(log_file_name), "log%03u.bin", file_number); + snprintf(log_file_path, sizeof(log_file_path), "%s/%s", log_dir, log_file_name); - if (!file_exist(log_file)) { + if (!file_exist(log_file_path)) { break; } file_number++; @@ -366,19 +368,19 @@ int open_logfile() if (file_number > MAX_NO_LOGFILE) { /* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */ - warnx("all %d possible files exist already.", MAX_NO_LOGFILE); + warnx("all %d possible files exist already", MAX_NO_LOGFILE); return -1; } } - int fd = open(log_file, O_CREAT | O_WRONLY | O_DSYNC); + int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC); if (fd < 0) { - warn("failed opening %s", log_file); - mavlink_log_info(mavlink_fd, "[sdlog2] failed opening %s", log_file); + warn("failed opening log: %s", log_file_name); + mavlink_log_info(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name); } else { - warnx("logging to: %s", log_file); - mavlink_log_info(mavlink_fd, "[sdlog2] log: %s", log_file); + warnx("log file: %s", log_file_name); + mavlink_log_info(mavlink_fd, "[sdlog2] log file: %s", log_file_name); } return fd; @@ -389,7 +391,7 @@ static void *logwriter_thread(void *arg) /* set name */ prctl(PR_SET_NAME, "sdlog2_writer", 0); - int log_fd = open_logfile(); + int log_fd = open_log_file(); if (log_fd < 0) return; @@ -477,7 +479,7 @@ static void *logwriter_thread(void *arg) void sdlog2_start_log() { - warnx("start logging."); + warnx("start logging"); mavlink_log_info(mavlink_fd, "[sdlog2] start logging"); /* create log dir if needed */ @@ -486,16 +488,6 @@ void sdlog2_start_log() errx(1, "error creating log dir"); } - const char *converter_in = "/etc/logging/conv.zip"; - char *converter_out = malloc(120); - sprintf(converter_out, "%s/conv.zip", log_dir); - - if (file_copy(converter_in, converter_out)) { - warnx("unable to copy conversion scripts"); - } - - free(converter_out); - /* initialize statistics counter */ log_bytes_written = 0; start_time = hrt_absolute_time(); @@ -524,7 +516,7 @@ void sdlog2_start_log() void sdlog2_stop_log() { - warnx("stop logging."); + warnx("stop logging"); mavlink_log_info(mavlink_fd, "[sdlog2] stop logging"); logging_enabled = false; @@ -632,7 +624,7 @@ int sdlog2_thread_main(int argc, char *argv[]) mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); if (mavlink_fd < 0) { - warnx("failed to open MAVLink log stream, start mavlink app first."); + warnx("failed to open MAVLink log stream, start mavlink app first"); } /* log buffer size */ @@ -689,32 +681,43 @@ int sdlog2_thread_main(int argc, char *argv[]) case '?': if (optopt == 'c') { - warnx("Option -%c requires an argument.", optopt); + warnx("option -%c requires an argument", optopt); } else if (isprint(optopt)) { - warnx("Unknown option `-%c'.", optopt); + warnx("unknown option `-%c'", optopt); } else { - warnx("Unknown option character `\\x%x'.", optopt); + warnx("unknown option character `\\x%x'", optopt); } default: sdlog2_usage("unrecognized flag"); - errx(1, "exiting."); + errx(1, "exiting"); } } gps_time = 0; - if (!file_exist(mountpoint)) { - errx(1, "logging mount point %s not present, exiting.", mountpoint); + /* create log root dir */ + int mkdir_ret = mkdir(log_root, S_IRWXU | S_IRWXG | S_IRWXO); + if (mkdir_ret != 0 && errno != EEXIST) { + err("failed creating log root dir: %s", log_root); } + /* copy conversion scripts */ + const char *converter_in = "/etc/logging/conv.zip"; + char *converter_out = malloc(64); + snprintf(converter_out, 64, "%s/conv.zip", log_root); + if (file_copy(converter_in, converter_out) != OK) { + warn("unable to copy conversion scripts"); + } + free(converter_out); + /* initialize log buffer with specified size */ - warnx("log buffer size: %i bytes.", log_buffer_size); + warnx("log buffer size: %i bytes", log_buffer_size); if (OK != logbuffer_init(&lb, log_buffer_size)) { - errx(1, "can't allocate log buffer, exiting."); + errx(1, "can't allocate log buffer, exiting"); } struct vehicle_status_s buf_status; @@ -935,7 +938,7 @@ int sdlog2_thread_main(int argc, char *argv[]) * differs from the number of messages in the above list. */ if (fdsc_count > fdsc) { - warn("WARNING: Not enough space for poll fds allocated. Check %s:%d.", __FILE__, __LINE__); + warn("WARNING: Not enough space for poll fds allocated. Check %s:%d", __FILE__, __LINE__); fdsc_count = fdsc; } @@ -978,7 +981,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* handle the poll result */ if (poll_ret < 0) { - warnx("ERROR: poll error, stop logging."); + warnx("ERROR: poll error, stop logging"); main_thread_should_exit = true; } else if (poll_ret > 0) { @@ -1337,7 +1340,7 @@ int sdlog2_thread_main(int argc, char *argv[]) free(lb.data); - warnx("exiting."); + warnx("exiting"); thread_running = false; @@ -1350,8 +1353,8 @@ void sdlog2_status() float mebibytes = kibibytes / 1024.0f; float seconds = ((float)(hrt_absolute_time() - start_time)) / 1000000.0f; - warnx("wrote %lu msgs, %4.2f MiB (average %5.3f KiB/s), skipped %lu msgs.", log_msgs_written, (double)mebibytes, (double)(kibibytes / seconds), log_msgs_skipped); - mavlink_log_info(mavlink_fd, "[sdlog2] wrote %lu msgs, skipped %lu msgs.", log_msgs_written, log_msgs_skipped); + warnx("wrote %lu msgs, %4.2f MiB (average %5.3f KiB/s), skipped %lu msgs", log_msgs_written, (double)mebibytes, (double)(kibibytes / seconds), log_msgs_skipped); + mavlink_log_info(mavlink_fd, "[sdlog2] wrote %lu msgs, skipped %lu msgs", log_msgs_written, log_msgs_skipped); } /** @@ -1370,7 +1373,7 @@ int file_copy(const char *file_old, const char *file_new) int ret = 0; if (source == NULL) { - warnx("failed opening input file to copy."); + warnx("failed opening input file to copy"); return 1; } @@ -1378,7 +1381,7 @@ int file_copy(const char *file_old, const char *file_new) if (target == NULL) { fclose(source); - warnx("failed to open output file to copy."); + warnx("failed to open output file to copy"); return 1; } @@ -1389,7 +1392,7 @@ int file_copy(const char *file_old, const char *file_new) ret = fwrite(buf, 1, nread, target); if (ret <= 0) { - warnx("error writing file."); + warnx("error writing file"); ret = 1; break; } -- cgit v1.2.3 From 47c226988ccf0e90bda9fe7c106bcdaf8b2e67fd Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 19 Jan 2014 22:56:57 +0100 Subject: sdlog2: code style fixed --- src/modules/sdlog2/sdlog2.c | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 656575573..2319dbb3b 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -301,12 +301,15 @@ int create_log_dir() int n = snprintf(log_dir, sizeof(log_dir), "%s/", log_root); strftime(log_dir + n, sizeof(log_dir) - n, "%Y-%m-%d", &t); mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO); + if (mkdir_ret == OK) { warnx("log dir created: %s", log_dir); + } else if (errno != EEXIST) { warn("failed creating new dir: %s", log_dir); return -1; } + } else { /* look for the next dir that does not exist */ while (dir_number <= MAX_NO_LOGFOLDER) { @@ -317,10 +320,12 @@ int create_log_dir() if (mkdir_ret == 0) { warnx("log dir created: %s", log_dir); break; + } else if (errno != EEXIST) { warn("failed creating new dir: %s", log_dir); return -1; } + /* dir exists already */ dir_number++; continue; @@ -352,8 +357,10 @@ int open_log_file() gmtime_r(&gps_time_sec, &t); strftime(log_file_name, sizeof(log_file_name), "%H_%M_%S.bin", &t); snprintf(log_file_path, sizeof(log_file_path), "%s/%s", log_dir, log_file_name); + } else { uint16_t file_number = 1; // start with file log001 + /* look for the next file that does not exist */ while (file_number <= MAX_NO_LOGFILE) { /* format log file path: e.g. /fs/microsd/sess001/log001.bin */ @@ -363,6 +370,7 @@ int open_log_file() if (!file_exist(log_file_path)) { break; } + file_number++; } @@ -378,6 +386,7 @@ int open_log_file() if (fd < 0) { warn("failed opening log: %s", log_file_name); mavlink_log_info(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name); + } else { warnx("log file: %s", log_file_name); mavlink_log_info(mavlink_fd, "[sdlog2] log file: %s", log_file_name); @@ -605,8 +614,8 @@ int write_parameters(int fd) } case PARAM_TYPE_FLOAT: - param_get(param, &value); - break; + param_get(param, &value); + break; default: break; @@ -700,6 +709,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* create log root dir */ int mkdir_ret = mkdir(log_root, S_IRWXU | S_IRWXG | S_IRWXO); + if (mkdir_ret != 0 && errno != EEXIST) { err("failed creating log root dir: %s", log_root); } @@ -708,9 +718,11 @@ int sdlog2_thread_main(int argc, char *argv[]) const char *converter_in = "/etc/logging/conv.zip"; char *converter_out = malloc(64); snprintf(converter_out, 64, "%s/conv.zip", log_root); + if (file_copy(converter_in, converter_out) != OK) { warn("unable to copy conversion scripts"); } + free(converter_out); /* initialize log buffer with specified size */ @@ -969,6 +981,7 @@ int sdlog2_thread_main(int argc, char *argv[]) gps_time = buf.gps_pos.time_gps_usec; } } + sdlog2_start_log(); } -- cgit v1.2.3 From 1e6d83fc9ea1c33b6a76623cc86a7889dc4f6c49 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 20 Jan 2014 13:10:37 +0100 Subject: Hotfix for IO battery voltage --- src/drivers/px4io/px4io.cpp | 1 + src/modules/sensors/sensors.cpp | 4 ++-- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index dede5976d..c8f77a611 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1302,6 +1302,7 @@ PX4IO::io_handle_battery(uint16_t vbatt, uint16_t ibatt) /* voltage is scaled to mV */ battery_status.voltage_v = vbatt / 1000.0f; + battery_status.voltage_filtered_v = vbatt / 1000.0f; /* ibatt contains the raw ADC count, as 12 bit ADC diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index ff6c5882e..9903a90a0 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1548,8 +1548,8 @@ Sensors::task_main() raw.adc_voltage_v[3] = 0.0f; memset(&_battery_status, 0, sizeof(_battery_status)); - _battery_status.voltage_v = 0.0f; - _battery_status.voltage_filtered_v = 0.0f; + _battery_status.voltage_v = -1.0f; + _battery_status.voltage_filtered_v = -1.0f; _battery_status.current_a = -1.0f; _battery_status.discharged_mah = -1.0f; -- cgit v1.2.3 From 5bc61c3c57164a7d2d118d3f7399a2a24e7199cd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 22 Jan 2014 08:30:48 +0100 Subject: S.BUS output disable cleanup --- src/drivers/boards/px4io-v2/board_config.h | 2 +- src/drivers/boards/px4io-v2/px4iov2_init.c | 2 -- src/modules/px4iofirmware/px4io.h | 2 +- 3 files changed, 2 insertions(+), 4 deletions(-) diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h index 8152ea4d4..ef9bb5cad 100644 --- a/src/drivers/boards/px4io-v2/board_config.h +++ b/src/drivers/boards/px4io-v2/board_config.h @@ -114,7 +114,7 @@ /* XXX these should be UART pins */ #define GPIO_SBUS_INPUT (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN11) #define GPIO_SBUS_OUTPUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) -#define GPIO_SBUS_OENABLE (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN4) +#define GPIO_SBUS_OENABLE (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN4) /* * High-resolution timer diff --git a/src/drivers/boards/px4io-v2/px4iov2_init.c b/src/drivers/boards/px4io-v2/px4iov2_init.c index ccd01edf5..9f8c0eeb2 100644 --- a/src/drivers/boards/px4io-v2/px4iov2_init.c +++ b/src/drivers/boards/px4io-v2/px4iov2_init.c @@ -124,8 +124,6 @@ __EXPORT void stm32_boardinitialize(void) stm32_configgpio(GPIO_ADC_VSERVO); stm32_configgpio(GPIO_SBUS_INPUT); /* xxx alternate function */ - - stm32_gpiowrite(GPIO_SBUS_OUTPUT, false); stm32_configgpio(GPIO_SBUS_OUTPUT); /* sbus output enable is active low - disable it by default */ diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 18c7468f6..d7f3e9adb 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -160,7 +160,7 @@ extern pwm_limit_t pwm_limit; # define PX4IO_RELAY_CHANNELS 0 # define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s)) -# define ENABLE_SBUS_OUT(_s) stm32_gpiowrite(GPIO_SBUS_OENABLE, (_s)) +# define ENABLE_SBUS_OUT(_s) stm32_gpiowrite(GPIO_SBUS_OENABLE, !(_s)) # define VDD_SERVO_FAULT (!stm32_gpioread(GPIO_SERVO_FAULT_DETECT)) -- cgit v1.2.3 From a794ee6c04550e1fd15b2c8bdf7835f6f0086251 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 22 Jan 2014 17:07:25 +0100 Subject: Hotfix for CMSIS exclude --- Documentation/Doxyfile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Documentation/Doxyfile b/Documentation/Doxyfile index 241702811..b674fbc48 100644 --- a/Documentation/Doxyfile +++ b/Documentation/Doxyfile @@ -599,7 +599,7 @@ RECURSIVE = YES # excluded from the INPUT source files. This way you can easily exclude a # subdirectory from a directory tree whose root is specified with the INPUT tag. -EXCLUDE = ../src/modules/mathlib/CMSIS \ +EXCLUDE = ../src/lib/mathlib/CMSIS \ ../src/modules/attitude_estimator_ekf/codegen # The EXCLUDE_SYMLINKS tag can be used select whether or not files or -- cgit v1.2.3 From 6c23e2f159b226fec394cca506e55b9b1d77365e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 22 Jan 2014 17:20:55 +0100 Subject: Added Doxygen main page --- src/mainpage.dox | 9 +++++++++ 1 file changed, 9 insertions(+) create mode 100644 src/mainpage.dox diff --git a/src/mainpage.dox b/src/mainpage.dox new file mode 100644 index 000000000..7ca410341 --- /dev/null +++ b/src/mainpage.dox @@ -0,0 +1,9 @@ +/** +\mainpage PX4 Autopilot Flight Control Stack and Middleware + +This software repository offers a middleware for micro aerial vehicles capable of running efficiently on a 168 MHz Cortex M4F processor and a state of the art flight control stack supporting multicopter and fixed wing aircraft. It can be easily used for experimental air (and ground) vehicles as well, as the application on a spherical blimp shows. + +http://pixhawk.org + + +*/ \ No newline at end of file -- cgit v1.2.3 From 1c40ce968a668a8d80535ab6799518da68fe0ac2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 23 Jan 2014 08:01:55 +0100 Subject: RC config params set to more useful default values - needs more testing --- src/modules/sensors/sensor_params.c | 63 +++++++++++++++++++++++++++++++++++-- 1 file changed, 61 insertions(+), 2 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index bbc84ef93..30659fd3a 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -381,14 +381,73 @@ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.00459340659f); #endif PARAM_DEFINE_FLOAT(BAT_C_SCALING, 0.0124); /* scaling for 3DR power brick */ +/** + * Roll control channel mapping. + * + * The channel index (starting from 1 for channel 1) indicates + * which channel should be used for reading roll inputs from. + * A value of zero indicates the switch is not assigned. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_MAP_ROLL, 1); + +/** + * Pitch control channel mapping. + * + * The channel index (starting from 1 for channel 1) indicates + * which channel should be used for reading pitch inputs from. + * A value of zero indicates the switch is not assigned. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_MAP_PITCH, 2); + +/** + * Throttle control channel mapping. + * + * The channel index (starting from 1 for channel 1) indicates + * which channel should be used for reading throttle inputs from. + * A value of zero indicates the switch is not assigned. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3); + +/** + * Yaw control channel mapping. + * + * The channel index (starting from 1 for channel 1) indicates + * which channel should be used for reading yaw inputs from. + * A value of zero indicates the switch is not assigned. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_MAP_YAW, 4); -PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5); +/** + * Mode switch channel mapping. + * + * This is the main flight mode selector. + * The channel index (starting from 1 for channel 1) indicates + * which channel should be used for deciding about the main mode. + * A value of zero indicates the switch is not assigned. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0); PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); -PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 6); +PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0); PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0); //PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); -- cgit v1.2.3 From ac37172b5280e661395035c17d2dfde91672b4b5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 23 Jan 2014 08:17:04 +0100 Subject: Hotfix: Prevent failures in boot handling due to missing microSD card logfile - we are not depending on the microSD any more completely --- ROMFS/px4fmu_common/init.d/rcS | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index a9c5c59ea..a2e562d6e 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -8,7 +8,6 @@ # set MODE autostart -set LOG_FILE /fs/microsd/bootlog.txt set RC_FILE /fs/microsd/etc/rc.txt set CONFIG_FILE /fs/microsd/etc/config.txt set EXTRAS_FILE /fs/microsd/etc/extras.txt @@ -21,10 +20,12 @@ set TUNE_OUT_ERROR ML< Date: Thu, 23 Jan 2014 08:25:37 +0100 Subject: Enable the PX4IO self check and debug interfaces. No reason to disable them, since they are runtime-configured (and needed, for the case of memory) --- src/modules/px4iofirmware/px4io.c | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index 150af35df..e9838d38c 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.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 @@ -228,7 +228,6 @@ user_start(int argc, char *argv[]) /* initialize PWM limit lib */ pwm_limit_init(&pwm_limit); -#if 0 /* not enough memory, lock down */ if (minfo.mxordblk < 500) { lowsyslog("ERR: not enough MEM"); @@ -245,7 +244,6 @@ user_start(int argc, char *argv[]) phase = !phase; usleep(300000); } -#endif /* * Run everything in a tight loop. @@ -275,7 +273,6 @@ user_start(int argc, char *argv[]) check_reboot(); -#if 0 /* check for debug activity */ show_debug_messages(); @@ -292,7 +289,6 @@ user_start(int argc, char *argv[]) (unsigned)minfo.mxordblk); last_debug_time = hrt_absolute_time(); } -#endif } } -- cgit v1.2.3 From f504863feef167c1ad493bf58c44420265892193 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 23 Jan 2014 08:25:56 +0100 Subject: Make startup less chatty --- 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 a2e562d6e..cbbdeeee8 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -181,7 +181,7 @@ then set IO_PRESENT yes else - echo "[init] PX4IO CRC failure, trying to update" + echo "[init] Trying to update" echo "PX4IO CRC failure" >> $LOG_FILE tone_alarm MLL32CP8MB -- cgit v1.2.3 From c3e4e4ee68f1f31d3ae281b0afb281fc7c58bc27 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 23 Jan 2014 08:26:53 +0100 Subject: Build fix, replaced usleep with up_udelay in memory lockdown state --- src/modules/px4iofirmware/px4io.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index e9838d38c..d8ebb43e9 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -242,7 +242,7 @@ user_start(int argc, char *argv[]) } phase = !phase; - usleep(300000); + up_udelay(300000); } /* -- cgit v1.2.3 From a0db455334c0928b953ab088e868e72fa3fc08f7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 23 Jan 2014 08:43:55 +0100 Subject: Cleanup wording in rcS --- 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 cbbdeeee8..edf2c77fb 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -182,7 +182,7 @@ then set IO_PRESENT yes else echo "[init] Trying to update" - echo "PX4IO CRC failure" >> $LOG_FILE + echo "PX4IO Trying to update" >> $LOG_FILE tone_alarm MLL32CP8MB -- cgit v1.2.3 From 8833f81b48aa738125b42a08aca05e3131cb8f8f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 23 Jan 2014 08:45:29 +0100 Subject: Do not make PX4IO start mandatory for forceupdate --- src/drivers/px4io/px4io.cpp | 37 ++++++++++++++++++++++++++++--------- 1 file changed, 28 insertions(+), 9 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 27ee1fb42..4844372b8 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2367,8 +2367,10 @@ start(int argc, char *argv[]) /* create the driver - it will set g_dev */ (void)new PX4IO(interface); - if (g_dev == nullptr) + if (g_dev == nullptr) { + delete interface; errx(1, "driver alloc failed"); + } if (OK != g_dev->init()) { delete g_dev; @@ -2769,18 +2771,35 @@ px4io_main(int argc, char *argv[]) } if (g_dev == nullptr) { warnx("px4io is not started, still attempting upgrade"); - } else { - uint16_t arg = atol(argv[2]); - int ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, arg); - if (ret != OK) { - printf("reboot failed - %d\n", ret); - exit(1); + + /* allocate the interface */ + device::Device *interface = get_interface(); + + /* create the driver - it will set g_dev */ + (void)new PX4IO(interface); + + if (g_dev == nullptr) { + delete interface; + errx(1, "driver alloc failed"); } - // tear down the px4io instance - delete g_dev; + if (OK != g_dev->init()) { + delete g_dev; + g_dev = nullptr; + errx(1, "driver init failed"); + } + } + + uint16_t arg = atol(argv[2]); + int ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, arg); + if (ret != OK) { + printf("reboot failed - %d\n", ret); + exit(1); } + // tear down the px4io instance + delete g_dev; + // upload the specified firmware const char *fn[2]; fn[0] = argv[3]; -- cgit v1.2.3 From 1ac8501d95f9d01bd2efc5b75373260dcc8d4530 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 22 Jan 2014 23:51:22 -0800 Subject: Clear the screen more properly. --- src/drivers/px4io/px4io.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 4844372b8..b85638788 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2646,17 +2646,17 @@ monitor(void) read(0, &c, 1); if (cancels-- == 0) { - printf("\033[H"); /* move cursor home and clear screen */ + printf("\033[2J\033[H"); /* move cursor home and clear screen */ exit(0); } } if (g_dev != nullptr) { - printf("\033[H"); /* move cursor home and clear screen */ + printf("\033[2J\033[H"); /* move cursor home and clear screen */ (void)g_dev->print_status(); (void)g_dev->print_debug(); - printf("[ Use 'px4io debug ' for more output. Hit three times to exit monitor mode ]\n"); + printf("\n\n\n[ Use 'px4io debug ' for more output. Hit three times to exit monitor mode ]\n"); } else { errx(1, "driver not loaded, exiting"); -- cgit v1.2.3 From 0994412ccae65349b144e0f29781889b18cd23ca Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 22 Jan 2014 23:51:53 -0800 Subject: Fix the initialisation and operation of the PX4IO ADC - now RSSI and VSERVO voltages should be read correctly. --- src/modules/px4iofirmware/adc.c | 53 +++++++++++++++++---------------------- src/modules/px4iofirmware/px4io.c | 3 +++ 2 files changed, 26 insertions(+), 30 deletions(-) diff --git a/src/modules/px4iofirmware/adc.c b/src/modules/px4iofirmware/adc.c index 81566eb2a..2f5908ac5 100644 --- a/src/modules/px4iofirmware/adc.c +++ b/src/modules/px4iofirmware/adc.c @@ -83,6 +83,14 @@ adc_init(void) { adc_perf = perf_alloc(PC_ELAPSED, "adc"); + /* put the ADC into power-down mode */ + rCR2 &= ~ADC_CR2_ADON; + up_udelay(10); + + /* bring the ADC out of power-down mode */ + rCR2 |= ADC_CR2_ADON; + up_udelay(10); + /* do calibration if supported */ #ifdef ADC_CR2_CAL rCR2 |= ADC_CR2_RSTCAL; @@ -96,41 +104,25 @@ adc_init(void) if (rCR2 & ADC_CR2_CAL) return -1; - #endif - /* arbitrarily configure all channels for 55 cycle sample time */ - rSMPR1 = 0b00000011011011011011011011011011; + /* + * Configure sampling time. + * + * For electrical protection reasons, we want to be able to have + * 10K in series with ADC inputs that leave the board. At 12MHz this + * means we need 28.5 cycles of sampling time (per table 43 in the + * datasheet). + */ + rSMPR1 = 0b00000000011011011011011011011011; rSMPR2 = 0b00011011011011011011011011011011; - /* XXX for F2/4, might want to select 12-bit mode? */ - rCR1 = 0; - - /* enable the temperature sensor / Vrefint channel if supported*/ - rCR2 = -#ifdef ADC_CR2_TSVREFE - /* enable the temperature sensor in CR2 */ - ADC_CR2_TSVREFE | -#endif - 0; - -#ifdef ADC_CCR_TSVREFE - /* enable temperature sensor in CCR */ - rCCR = ADC_CCR_TSVREFE; -#endif + rCR2 |= ADC_CR2_TSVREFE; /* enable the temperature sensor / Vrefint channel */ /* configure for a single-channel sequence */ rSQR1 = 0; rSQR2 = 0; - rSQR3 = 0; /* will be updated with the channel each tick */ - - /* power-cycle the ADC and turn it on */ - rCR2 &= ~ADC_CR2_ADON; - up_udelay(10); - rCR2 |= ADC_CR2_ADON; - up_udelay(10); - rCR2 |= ADC_CR2_ADON; - up_udelay(10); + rSQR3 = 0; /* will be updated with the channel at conversion time */ return 0; } @@ -141,11 +133,12 @@ adc_init(void) uint16_t adc_measure(unsigned channel) { + perf_begin(adc_perf); /* clear any previous EOC */ - if (rSR & ADC_SR_EOC) - rSR &= ~ADC_SR_EOC; + rSR = 0; + (void)rDR; /* run a single conversion right now - should take about 60 cycles (a few microseconds) max */ rSQR3 = channel; @@ -158,7 +151,6 @@ adc_measure(unsigned channel) /* never spin forever - this will give a bogus result though */ if (hrt_elapsed_time(&now) > 100) { - debug("adc timeout"); perf_end(adc_perf); return 0xffff; } @@ -166,6 +158,7 @@ adc_measure(unsigned channel) /* read the result and clear EOC */ uint16_t result = rDR; + rSR = 0; perf_end(adc_perf); return result; diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index d8ebb43e9..d03b64809 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -210,6 +210,9 @@ user_start(int argc, char *argv[]) /* initialise the control inputs */ controls_init(); + /* set up the ADC */ + adc_init(); + /* start the FMU interface */ interface_init(); -- cgit v1.2.3 From d77a15e94fd024633661eb92f72455d737a0aa84 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 23 Jan 2014 09:16:40 +0100 Subject: Last small fixes to IO driver to support updates with and without switch pressed and with and without px4io start call before the forceupdate call --- src/drivers/px4io/px4io.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 4844372b8..879e19973 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2784,9 +2784,7 @@ px4io_main(int argc, char *argv[]) } if (OK != g_dev->init()) { - delete g_dev; - g_dev = nullptr; - errx(1, "driver init failed"); + warnx("driver init failed, still trying.."); } } -- cgit v1.2.3 From 2aa76f1a3c4eb99074b38d287e0f18a98973671d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 23 Jan 2014 09:17:46 +0100 Subject: Fixes to memory check handling, split out switch handling to allow separate initialization --- src/modules/px4iofirmware/px4io.c | 41 ++++++++++++++++++++++++++++---------- src/modules/px4iofirmware/px4io.h | 1 + src/modules/px4iofirmware/safety.c | 8 ++++++-- 3 files changed, 37 insertions(+), 13 deletions(-) diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index d8ebb43e9..dba35752e 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -228,23 +228,42 @@ user_start(int argc, char *argv[]) /* initialize PWM limit lib */ pwm_limit_init(&pwm_limit); - /* not enough memory, lock down */ - if (minfo.mxordblk < 500) { + /* + * P O L I C E L I G H T S + * + * Not enough memory, lock down. + * + * We might need to allocate mixers later, and this will + * ensure that a developer doing a change will notice + * that he just burned the remaining RAM with static + * allocations. We don't want him to be able to + * get past that point. This needs to be clearly + * documented in the dev guide. + * + */ + if (minfo.mxordblk < 600) { + lowsyslog("ERR: not enough MEM"); bool phase = false; - if (phase) { - LED_AMBER(true); - LED_BLUE(false); - } else { - LED_AMBER(false); - LED_BLUE(true); - } + while (true) { - phase = !phase; - up_udelay(300000); + if (phase) { + LED_AMBER(true); + LED_BLUE(false); + } else { + LED_AMBER(false); + LED_BLUE(true); + } + up_udelay(250000); + + phase = !phase; + } } + /* Start the failsafe led init */ + failsafe_led_init(); + /* * Run everything in a tight loop. */ diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index d7f3e9adb..bffbc0ce2 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -184,6 +184,7 @@ extern void mixer_handle_text(const void *buffer, size_t length); * Safety switch/LED. */ extern void safety_init(void); +extern void failsafe_led_init(void); /** * FMU communications diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c index cdb54a80a..2ce479ffd 100644 --- a/src/modules/px4iofirmware/safety.c +++ b/src/modules/px4iofirmware/safety.c @@ -84,7 +84,11 @@ safety_init(void) { /* arrange for the button handler to be called at 10Hz */ hrt_call_every(&arming_call, 1000, 100000, safety_check_button, NULL); +} +void +failsafe_led_init(void) +{ /* arrange for the failsafe blinker to be called at 8Hz */ hrt_call_every(&failsafe_call, 1000, 125000, failsafe_blink, NULL); } @@ -165,8 +169,8 @@ failsafe_blink(void *arg) /* indicate that a serious initialisation error occured */ if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)) { LED_AMBER(true); - return; - } + return; + } static bool failsafe = false; -- cgit v1.2.3 From 9e72e726442198feb6b4d3a725c8195ef55ffe55 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 23 Jan 2014 09:29:59 +0100 Subject: Make SBUS switching conditional to be friendly to IO v1 --- src/modules/px4iofirmware/controls.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 5859f768b..5e2c92bf4 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -123,7 +123,9 @@ controls_tick() { /* switch S.Bus output pin as needed */ if (sbus_status != (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS)) { + #ifdef ENABLE_SBUS_OUT ENABLE_SBUS_OUT((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS)); + #endif } perf_end(c_gather_sbus); -- cgit v1.2.3 From 6a40acdbdc6abe57fb202a02e6ab6fa8c90698a9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 23 Jan 2014 09:58:22 +0100 Subject: Fixed PPM warning to be only printed with PPM inputs enabled --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 08ff3792f..a54bb3964 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1810,7 +1810,7 @@ PX4IO::print_status() printf("\n"); - if (raw_inputs > 0) { + if ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM)) { int frame_len = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_RC_DATA); printf("RC data (PPM frame len) %u us\n", frame_len); -- cgit v1.2.3 From 1afe7f2c50016fbb39cb532252be171ba03ad357 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 23 Jan 2014 18:39:32 +0100 Subject: Added tune on IO upgrade error --- ROMFS/px4fmu_common/init.d/rcS | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index edf2c77fb..6f4e1f3b5 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -199,10 +199,12 @@ then else echo "[init] ERROR: PX4IO update failed" echo "PX4IO update failed" >> $LOG_FILE + tone_alarm $TUNE_OUT_ERROR fi else echo "[init] ERROR: PX4IO update failed" echo "PX4IO update failed" >> $LOG_FILE + tone_alarm $TUNE_OUT_ERROR fi fi -- cgit v1.2.3 From dda50c62bfd26463718f50d2f9c1cdbecc7de4ac Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 22 Jan 2014 16:33:35 +1100 Subject: hmc5883: much faster calibration code with bug fixes this fixes two bugs in "hmc5883 calibrate" and also makes it much faster, so it can be run on every boot. It now uses the correct 2.5Ga range when calibrating, and fixes the expected values for X/Y/Z axes The basic calibration approach is similar to the APM2 driver, waiting for 10 good samples after discarding some initial samples. That allows the calibration to run fast enough that it can be done on every boot without causing too much boot delay. --- src/drivers/hmc5883/hmc5883.cpp | 160 ++++++++++++++++++++++------------------ 1 file changed, 87 insertions(+), 73 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index d3b99ae66..9b9c11af2 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -849,42 +849,24 @@ HMC5883::collect() /* scale values for output */ - /* - * 1) Scale raw value to SI units using scaling from datasheet. - * 2) Subtract static offset (in SI units) - * 3) Scale the statically calibrated values with a linear - * dynamically obtained factor - * - * Note: the static sensor offset is the number the sensor outputs - * at a nominally 'zero' input. Therefore the offset has to - * be subtracted. - * - * Example: A gyro outputs a value of 74 at zero angular rate - * the offset is 74 from the origin and subtracting - * 74 from all measurements centers them around zero. - */ - #ifdef PX4_I2C_BUS_ONBOARD if (_bus == PX4_I2C_BUS_ONBOARD) { - /* to align the sensor axes with the board, x and y need to be flipped */ - new_report.x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; - /* flip axes and negate value for y */ - new_report.y = ((-report.x * _range_scale) - _scale.y_offset) * _scale.y_scale; - /* z remains z */ - new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; - } else { -#endif - /* the standard external mag by 3DR has x pointing to the right, y pointing backwards, and z down, - * therefore switch x and y and invert y */ - new_report.x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; - /* flip axes and negate value for y */ - new_report.y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale; - /* z remains z */ - new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; -#ifdef PX4_I2C_BUS_ONBOARD - } + // convert onboard so it matches offboard for the + // scaling below + report.y = -report.y; + report.x = -report.x; + } #endif + /* the standard external mag by 3DR has x pointing to the + * right, y pointing backwards, and z down, therefore switch x + * and y and invert y */ + new_report.x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; + /* flip axes and negate value for y */ + new_report.y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale; + /* z remains z */ + new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; + if (_mag_topic != -1) { /* publish it */ orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report); @@ -910,6 +892,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) struct mag_report report; ssize_t sz; int ret = 1; + uint8_t good_count = 0; // XXX do something smarter here int fd = (int)enable; @@ -932,31 +915,16 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) 1.0f, }; - float avg_excited[3] = {0.0f, 0.0f, 0.0f}; - unsigned i; + float sum_excited[3] = {0.0f, 0.0f, 0.0f}; - warnx("starting mag scale calibration"); + /* expected axis scaling. The datasheet says that 766 will + * be places in the X and Y axes and 713 in the Z + * axis. Experiments show that in fact 766 is placed in X, + * and 713 in Y and Z. This is relative to a base of 660 + * LSM/Ga, giving 1.16 and 1.08 */ + float expected_cal[3] = { 1.16f, 1.08f, 1.08f }; - /* do a simple demand read */ - sz = read(filp, (char *)&report, sizeof(report)); - - if (sz != sizeof(report)) { - warn("immediate read failed"); - ret = 1; - goto out; - } - - warnx("current measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); - warnx("time: %lld", report.timestamp); - warnx("sampling 500 samples for scaling offset"); - - /* set the queue depth to 10 */ - /* don't do this for now, it can lead to a crash in start() respectively work_queue() */ -// if (OK != ioctl(filp, SENSORIOCSQUEUEDEPTH, 10)) { -// warn("failed to set queue depth"); -// ret = 1; -// goto out; -// } + warnx("starting mag scale calibration"); /* start the sensor polling at 50 Hz */ if (OK != ioctl(filp, SENSORIOCSPOLLRATE, 50)) { @@ -965,8 +933,9 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) goto out; } - /* Set to 2.5 Gauss */ - if (OK != ioctl(filp, MAGIOCSRANGE, 2)) { + /* Set to 2.5 Gauss. We ask for 3 to get the right part of + * the chained if statement above. */ + if (OK != ioctl(filp, MAGIOCSRANGE, 3)) { warnx("failed to set 2.5 Ga range"); ret = 1; goto out; @@ -990,8 +959,8 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) goto out; } - /* read the sensor 10x and report each value */ - for (i = 0; i < 500; i++) { + // discard 10 samples to let the sensor settle + for (uint8_t i = 0; i < 10; i++) { struct pollfd fds; /* wait for data to be ready */ @@ -1009,32 +978,69 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) if (sz != sizeof(report)) { warn("periodic read failed"); + ret = -EIO; goto out; + } + } - } else { - avg_excited[0] += report.x; - avg_excited[1] += report.y; - avg_excited[2] += report.z; + /* read the sensor up to 50x, stopping when we have 10 good values */ + for (uint8_t i = 0; i < 50 && good_count < 10; i++) { + struct pollfd fds; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = ::poll(&fds, 1, 2000); + + if (ret != 1) { + warn("timed out waiting for sensor data"); + goto out; + } + + /* now go get it */ + sz = ::read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + warn("periodic read failed"); + ret = -EIO; + goto out; + } + float cal[3] = {fabsf(expected_cal[0] / report.x), + fabsf(expected_cal[1] / report.y), + fabsf(expected_cal[2] / report.z)}; + + if (cal[0] > 0.7f && cal[0] < 1.35f && + cal[1] > 0.7f && cal[1] < 1.35f && + cal[2] > 0.7f && cal[2] < 1.35f) { + good_count++; + sum_excited[0] += cal[0]; + sum_excited[1] += cal[1]; + sum_excited[2] += cal[2]; } //warnx("periodic read %u", i); //warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); + //warnx("cal: %.6f %.6f %.6f", (double)cal[0], (double)cal[1], (double)cal[2]); } - avg_excited[0] /= i; - avg_excited[1] /= i; - avg_excited[2] /= i; + if (good_count < 5) { + warn("failed calibration"); + ret = -EIO; + goto out; + } - warnx("done. Performed %u reads", i); - warnx("measurement avg: %.6f %.6f %.6f", (double)avg_excited[0], (double)avg_excited[1], (double)avg_excited[2]); +#if 0 + warnx("measurement avg: %.6f %.6f %.6f", + (double)sum_excited[0]/good_count, + (double)sum_excited[1]/good_count, + (double)sum_excited[2]/good_count); +#endif float scaling[3]; - /* calculate axis scaling */ - scaling[0] = fabsf(1.16f / avg_excited[0]); - /* second axis inverted */ - scaling[1] = fabsf(1.16f / -avg_excited[1]); - scaling[2] = fabsf(1.08f / avg_excited[2]); + scaling[0] = sum_excited[0] / good_count; + scaling[1] = sum_excited[1] / good_count; + scaling[2] = sum_excited[2] / good_count; warnx("axes scaling: %.6f %.6f %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]); @@ -1165,6 +1171,8 @@ int HMC5883::set_excitement(unsigned enable) conf_reg &= ~0x03; } + // ::printf("set_excitement enable=%d regA=0x%x\n", (int)enable, (unsigned)conf_reg); + ret = write_reg(ADDR_CONF_A, conf_reg); if (OK != ret) @@ -1173,6 +1181,8 @@ int HMC5883::set_excitement(unsigned enable) uint8_t conf_reg_ret; read_reg(ADDR_CONF_A, conf_reg_ret); + //print_info(); + return !(conf_reg == conf_reg_ret); } @@ -1211,6 +1221,10 @@ HMC5883::print_info() perf_print_counter(_comms_errors); perf_print_counter(_buffer_overflows); printf("poll interval: %u ticks\n", _measure_ticks); + printf("offsets (%.2f %.2f %.2f)\n", (double)_scale.x_offset, (double)_scale.y_offset, (double)_scale.z_offset); + printf("scaling (%.2f %.2f %.2f) 1/range_scale %.2f range_ga %.2f\n", + (double)_scale.x_scale, (double)_scale.y_scale, (double)_scale.z_scale, + (double)1.0/_range_scale, (double)_range_ga); _reports->print_info("report queue"); } -- cgit v1.2.3 From 4524fe3e4888d569f855d1e7a82c8d5116636a0a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 20 Jan 2014 16:20:43 +1100 Subject: px4fmu: added PWM_SERVO_SET_COUNT API this allows the balance between PWM channels and GPIOs to be changed after the main flight code has started, which makes it possible to change the balance with a parameter in APM --- src/drivers/drv_pwm_output.h | 4 ++++ src/drivers/px4fmu/fmu.cpp | 35 ++++++++++++++++++++++++++++++++++- 2 files changed, 38 insertions(+), 1 deletion(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 51f916f37..88da94b1e 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -189,6 +189,10 @@ ORB_DECLARE(output_pwm); /** get the maximum PWM value the output will send */ #define PWM_SERVO_GET_MAX_PWM _IOC(_PWM_SERVO_BASE, 19) +/** set the number of servos in (unsigned)arg - allows change of + * split between servos and GPIO */ +#define PWM_SERVO_SET_COUNT _IOC(_PWM_SERVO_BASE, 20) + /** set a single servo to a specific value */ #define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 4b1b0ed0b..c067d363b 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1006,6 +1006,40 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; + case PWM_SERVO_SET_COUNT: { + /* change the number of outputs that are enabled for + * PWM. This is used to change the split between GPIO + * and PWM under control of the flight config + * parameters. Note that this does not allow for + * changing a set of pins to be used for serial on + * FMUv1 + */ + switch (arg) { + case 0: + set_mode(MODE_NONE); + break; + + case 2: + set_mode(MODE_2PWM); + break; + + case 4: + set_mode(MODE_4PWM); + break; + +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) + case 6: + set_mode(MODE_6PWM); + break; +#endif + + default: + ret = -EINVAL; + break; + } + break; + } + case MIXERIOCRESET: if (_mixers != nullptr) { delete _mixers; @@ -1443,7 +1477,6 @@ void sensor_reset(int ms) { int fd; - int ret; fd = open(PX4FMU_DEVICE_PATH, O_RDWR); -- cgit v1.2.3 From 0c116e8de5c5c958b9463f147576f3e0377c4c00 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 23 Jan 2014 22:10:03 +0100 Subject: Implemented S.Bus 2 decoding support --- src/modules/px4iofirmware/sbus.c | 27 +++++++++++++++++++++++++-- 1 file changed, 25 insertions(+), 2 deletions(-) diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 11ccd7356..4efa72dc1 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.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 @@ -218,7 +218,30 @@ static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_values) { /* check frame boundary markers to avoid out-of-sync cases */ - if ((frame[0] != 0x0f) || (frame[24] != 0x00)) { + if ((frame[0] != 0x0f)) { + sbus_frame_drops++; + return false; + } + + switch (frame[24]) { + case 0x00: + /* this is S.BUS 1 */ + break; + case 0x03: + /* S.BUS 2 SLOT0: RX battery and external voltage */ + break; + case 0x83: + /* S.BUS 2 SLOT1 */ + break; + case 0x43: + case 0xC3: + case 0x23: + case 0xA3: + case 0x63: + case 0xE3: + break; + default: + /* we expect one of these, else abort */ sbus_frame_drops++; return false; } -- cgit v1.2.3 From 8bdbce5fe2893353bf9582294c28ab2831f96a9d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 23 Jan 2014 22:27:04 +0100 Subject: We do not know all secret S.BUS2 codes yet --- src/modules/px4iofirmware/sbus.c | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 4efa72dc1..495447740 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -241,9 +241,8 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint case 0xE3: break; default: - /* we expect one of these, else abort */ - sbus_frame_drops++; - return false; + /* we expect one of the bits above, but there are some we don't know yet */ + break; } /* we have received something we think is a frame */ -- cgit v1.2.3 From d8c1131f1e8e61bcb15b0faa36de1bba00e9716d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 24 Jan 2014 11:06:33 +1100 Subject: px4io: improved reliability of forceupdate re-starting px4io this adds a 0.1s delay after update to give px4io time to boot. It removes the need for the user to reboot after an IO update --- src/drivers/px4io/px4io_uploader.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index 41f93a8ee..d9f991210 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -51,6 +51,7 @@ #include #include #include +#include #include @@ -226,6 +227,11 @@ PX4IO_Uploader::upload(const char *filenames[]) close(_fw_fd); close(_io_fd); _io_fd = -1; + + // sleep for enough time for the IO chip to boot. This makes + // forceupdate more reliably startup IO again after update + up_udelay(100*1000); + return ret; } -- cgit v1.2.3 From 65118f0c2ef6e4305259a35751c8cb92d751b671 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 24 Jan 2014 17:26:13 +0100 Subject: Disable debug in the airspeed sensor driver - prevents console spam if it fails (and on probing during startup) --- src/drivers/airspeed/airspeed.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 5e45cc936..f73a3ef01 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -91,7 +91,7 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) : _comms_errors(perf_alloc(PC_COUNT, "airspeed_comms_errors")) { // enable debug() calls - _debug_enabled = true; + _debug_enabled = false; // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); -- cgit v1.2.3 From 4f78c3e60596d1b596e5ebcf4bb4e101a5b356e7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 24 Jan 2014 17:27:28 +0100 Subject: Disable PX4IO debug - spams console on comms failure. Each command does report the failure separately, so we get a better feedback level without the spam. --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index a54bb3964..519aa96eb 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -505,7 +505,7 @@ PX4IO::PX4IO(device::Device *interface) : /* open MAVLink text channel */ _mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0); - _debug_enabled = true; + _debug_enabled = false; _servorail_status.rssi_v = 0; } -- cgit v1.2.3 From 2f968357a368ee59f53d75119b893487abd3883b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 24 Jan 2014 17:28:04 +0100 Subject: Make the protocol version more descriptive - helps to understand when / how px4io detect fails. --- src/drivers/px4io/px4io.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 519aa96eb..6f1323fce 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -580,6 +580,12 @@ PX4IO::init() /* get some parameters */ unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION); + if (protocol == _io_reg_get_error) { + log("failed to communicate with IO"); + mavlink_log_emergency(_mavlink_fd, "[IO] failed to communicate with IO, abort."); + return -1; + } + if (protocol != PX4IO_PROTOCOL_VERSION) { log("protocol/firmware mismatch"); mavlink_log_emergency(_mavlink_fd, "[IO] protocol/firmware mismatch, abort."); -- cgit v1.2.3 From 1960f7d6c5c502860ad4f2520eae364a4abfe9e7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 24 Jan 2014 17:29:27 +0100 Subject: Initialize null pointers correctly, always set the pointer to null after deletes. Remove some verbosity from startup and do not try to initialise IO when we just want to reboot it into the bootloader. --- src/drivers/px4io/px4io.cpp | 39 ++++++++++++++++++++++++++++----------- 1 file changed, 28 insertions(+), 11 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 6f1323fce..df847a64d 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -451,7 +451,7 @@ private: namespace { -PX4IO *g_dev; +PX4IO *g_dev = nullptr; } @@ -780,8 +780,6 @@ PX4IO::task_main() hrt_abstime poll_last = 0; hrt_abstime orb_check_last = 0; - log("starting"); - _thread_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0); /* @@ -815,8 +813,6 @@ PX4IO::task_main() fds[0].fd = _t_actuator_controls_0; fds[0].events = POLLIN; - log("ready"); - /* lock against the ioctl handler */ lock(); @@ -1679,7 +1675,18 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) total_len++; } - int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2); + int ret; + + for (int i = 0; i < 30; i++) { + /* failed, but give it a 2nd shot */ + ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2); + + if (ret) { + usleep(333); + } else { + break; + } + } /* print mixer chunk */ if (debuglevel > 5 || ret) { @@ -1703,7 +1710,18 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) msg->text[0] = '\n'; msg->text[1] = '\0'; - int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, (sizeof(px4io_mixdata) + 2) / 2); + int ret; + + for (int i = 0; i < 30; i++) { + /* failed, but give it a 2nd shot */ + ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, (sizeof(px4io_mixdata) + 2) / 2); + + if (ret) { + usleep(333); + } else { + break; + } + } if (ret) return ret; @@ -2705,6 +2723,7 @@ px4io_main(int argc, char *argv[]) printf("[px4io] loaded, detaching first\n"); /* stop the driver */ delete g_dev; + g_dev = nullptr; } PX4IO_Uploader *up; @@ -2788,10 +2807,6 @@ px4io_main(int argc, char *argv[]) delete interface; errx(1, "driver alloc failed"); } - - if (OK != g_dev->init()) { - warnx("driver init failed, still trying.."); - } } uint16_t arg = atol(argv[2]); @@ -2803,6 +2818,7 @@ px4io_main(int argc, char *argv[]) // tear down the px4io instance delete g_dev; + g_dev = nullptr; // upload the specified firmware const char *fn[2]; @@ -2861,6 +2877,7 @@ px4io_main(int argc, char *argv[]) /* stop the driver */ delete g_dev; + g_dev = nullptr; exit(0); } -- cgit v1.2.3 From 73a483c2657d97619021b85759bc742f637cfff4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 24 Jan 2014 17:30:40 +0100 Subject: Finally fix the timing race between the IO driver, IO uploader and the on-IO firmware by making the uploader tolerant of timing offsets. --- src/drivers/px4io/px4io_uploader.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index d9f991210..dd8abbac5 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -121,8 +121,15 @@ PX4IO_Uploader::upload(const char *filenames[]) cfsetspeed(&t, 115200); tcsetattr(_io_fd, TCSANOW, &t); - /* look for the bootloader */ - ret = sync(); + /* look for the bootloader for 150 ms */ + for (int i = 0; i < 15; i++) { + ret = sync(); + if (ret == OK) { + break; + } else { + usleep(10000); + } + } if (ret != OK) { /* this is immediately fatal */ -- cgit v1.2.3 From 92a6c7d7344ae0a463e0c04c3b5bc6cf8f4ddc53 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 24 Jan 2014 17:33:04 +0100 Subject: Set timeouts back to short, now that we have multiple tries in the uploader. This ensures we try often enough in the 200 ms IO bootloader wait phase to hit it. --- src/drivers/px4io/uploader.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/uploader.h b/src/drivers/px4io/uploader.h index 22387a3e2..55f63eef9 100644 --- a/src/drivers/px4io/uploader.h +++ b/src/drivers/px4io/uploader.h @@ -91,7 +91,7 @@ private: void drain(); int send(uint8_t c); int send(uint8_t *p, unsigned count); - int get_sync(unsigned timeout = 1000); + int get_sync(unsigned timeout = 40); int sync(); int get_info(int param, uint32_t &val); int erase(); -- cgit v1.2.3 From c5cb3cfd2187c82b11bb1f12d644e77ecd807efc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 24 Jan 2014 17:34:42 +0100 Subject: Make the IO mixer upload report not only a global success / fail flag, but on transfer basis. Also use a crude lock to avoid updating the mixer while it runs (we have no proper mutexes on IO, and this is a pure read/write locking case with two locks, which should make the execution even with this crude approach thread-safe). --- src/modules/px4iofirmware/mixer.cpp | 30 +++++++++++++++++++++++------- 1 file changed, 23 insertions(+), 7 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index e55ef784a..2e79f0ac6 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.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 @@ -71,6 +71,7 @@ extern "C" { static bool mixer_servos_armed = false; static bool should_arm = false; static bool should_always_enable_pwm = false; +static volatile bool in_mixer = false; /* selected control values and count for mixing */ enum mixer_source { @@ -95,6 +96,7 @@ static void mixer_set_failsafe(); void mixer_tick(void) { + /* check that we are receiving fresh data from the FMU */ if (hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) { @@ -199,13 +201,17 @@ mixer_tick(void) } - } else if (source != MIX_NONE) { + } else if (source != MIX_NONE && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { float outputs[PX4IO_SERVO_COUNT]; unsigned mixed; /* mix */ + + /* poor mans mutex */ + in_mixer = true; mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT); + in_mixer = false; 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); @@ -297,12 +303,17 @@ mixer_callback(uintptr_t handle, static char mixer_text[256]; /* large enough for one mixer */ static unsigned mixer_text_length = 0; -void +int mixer_handle_text(const void *buffer, size_t length) { /* do not allow a mixer change while safety off */ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) { - return; + return 1; + } + + /* abort if we're in the mixer */ + if (in_mixer) { + return 1; } px4io_mixdata *msg = (px4io_mixdata *)buffer; @@ -310,7 +321,7 @@ mixer_handle_text(const void *buffer, size_t length) isr_debug(2, "mix txt %u", length); if (length < sizeof(px4io_mixdata)) - return; + return 0; unsigned text_length = length - sizeof(px4io_mixdata); @@ -328,13 +339,16 @@ mixer_handle_text(const void *buffer, size_t length) case F2I_MIXER_ACTION_APPEND: isr_debug(2, "append %d", length); + /* disable mixing during the update */ + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; + /* check for overflow - this would be really fatal */ if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) { r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; - return; + return 0; } - /* append mixer text and nul-terminate */ + /* append mixer text and nul-terminate, guard against overflow */ memcpy(&mixer_text[mixer_text_length], msg->text, text_length); mixer_text_length += text_length; mixer_text[mixer_text_length] = '\0'; @@ -369,6 +383,8 @@ mixer_handle_text(const void *buffer, size_t length) break; } + + return 0; } static void -- cgit v1.2.3 From 15f8e5acf12125eb4fb7b3d5d530b3e27c25f34c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 24 Jan 2014 17:35:29 +0100 Subject: Make in the comments explicit that we don’t do anything here under normal circumstances to make it less tempting to comment out the helpful debug tools in this section. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/modules/px4iofirmware/px4io.c | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index e2da35939..d4c25911e 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -295,10 +295,12 @@ user_start(int argc, char *argv[]) check_reboot(); - /* check for debug activity */ + /* check for debug activity (default: none) */ show_debug_messages(); - /* post debug state at ~1Hz */ + /* post debug state at ~1Hz - this is via an auxiliary serial port + * DEFAULTS TO OFF! + */ if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) { struct mallinfo minfo = mallinfo(); -- cgit v1.2.3 From 33688fec9c66692e88a1b328fd022adc6e906853 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 24 Jan 2014 17:36:13 +0100 Subject: Make the sensors app less verbose --- src/modules/sensors/sensors.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 9903a90a0..23f20b0cb 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -797,7 +797,6 @@ Sensors::accel_init() #endif - warnx("using system accel"); close(fd); } } @@ -837,7 +836,6 @@ Sensors::gyro_init() #endif - warnx("using system gyro"); close(fd); } } @@ -1507,9 +1505,6 @@ void Sensors::task_main() { - /* inform about start */ - warnx("Initializing.."); - /* start individual sensors */ accel_init(); gyro_init(); -- cgit v1.2.3 From bd15653b173029dfc12c3d4a73b897570e0867c0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 24 Jan 2014 17:37:01 +0100 Subject: Use the proper status registers for locking out from mixer updates and return the value of the mixer change. --- src/modules/px4iofirmware/registers.c | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index ad4473073..2c437d2c0 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -382,7 +382,10 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* handle text going to the mixer parser */ case PX4IO_PAGE_MIXERLOAD: - mixer_handle_text(values, num_values * sizeof(*values)); + if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) || + (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { + return mixer_handle_text(values, num_values * sizeof(*values)); + } break; default: @@ -509,8 +512,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_P_SETUP_REBOOT_BL: if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) || - (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) || - (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { // don't allow reboot while armed break; } @@ -540,8 +542,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) * do not allow a RC config change while outputs armed */ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) || - (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) || - (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { break; } -- cgit v1.2.3 From 880342b9c1e90e0c22180d7fe1411d0988d97a49 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 24 Jan 2014 17:37:34 +0100 Subject: Missing header for mixer status change. --- src/modules/px4iofirmware/px4io.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index bffbc0ce2..393e0560e 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -178,7 +178,7 @@ extern pwm_limit_t pwm_limit; * Mixer */ extern void mixer_tick(void); -extern void mixer_handle_text(const void *buffer, size_t length); +extern int mixer_handle_text(const void *buffer, size_t length); /** * Safety switch/LED. -- cgit v1.2.3 From e07d91613b0a90fd4a9ce8c2e10d3bff8a1ebc44 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 24 Jan 2014 18:24:54 +0100 Subject: Remove unused field --- src/modules/px4iofirmware/safety.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c index 2ce479ffd..ff2e4af6e 100644 --- a/src/modules/px4iofirmware/safety.c +++ b/src/modules/px4iofirmware/safety.c @@ -45,7 +45,6 @@ #include "px4io.h" static struct hrt_call arming_call; -static struct hrt_call heartbeat_call; static struct hrt_call failsafe_call; /* -- cgit v1.2.3 From f2f94f0f176b9a2d818a0849eae18f018d7fb5a9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 25 Jan 2014 23:03:21 +0100 Subject: IO driver: Variable name and comment cleanup, no binary / functionality changes. --- src/drivers/px4io/px4io.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index df847a64d..e83fa68d5 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -444,7 +444,7 @@ private: * @param vservo vservo register * @param vrssi vrssi register */ - void io_handle_vservo(uint16_t vbatt, uint16_t ibatt); + void io_handle_vservo(uint16_t vservo, uint16_t vrssi); }; @@ -1357,7 +1357,10 @@ PX4IO::io_get_status() uint16_t regs[6]; int ret; - /* get STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT in that order */ + /* get + * STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT, + * STATUS_VSERVO, STATUS_VRSSI, STATUS_PRSSI + * in that order */ ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, ®s[0], sizeof(regs) / sizeof(regs[0])); if (ret != OK) -- cgit v1.2.3 From bafcbd99a695a3c4d478fb58e1d53940f331392f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 25 Jan 2014 23:04:16 +0100 Subject: Stop setting RSSI by cross-reading servo status. --- src/drivers/px4io/px4io.cpp | 7 ------- 1 file changed, 7 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index e83fa68d5..4bdf530a7 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1457,13 +1457,6 @@ PX4IO::io_publish_raw_rc() rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN; } - /* set RSSI */ - - if (rc_val.input_source != RC_INPUT_SOURCE_PX4IO_SBUS) { - // XXX the correct scaling needs to be validated here - rc_val.rssi = (_servorail_status.rssi_v / 3.3f) * UINT8_MAX; - } - /* lazily advertise on first publication */ if (_to_input_rc == 0) { _to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_val); -- cgit v1.2.3 From aff11d6d8611d5e5b72742d2bee4132168fefa72 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 25 Jan 2014 23:06:53 +0100 Subject: IO firmware: Use right base reg value - since the wrong one had the same value this hasn’t been an issue, but it would have become one once one of them changed. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/modules/px4iofirmware/px4io.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 393e0560e..ee9d5b0c4 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -97,7 +97,7 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */ #define r_raw_rc_count r_page_raw_rc_input[PX4IO_P_RAW_RC_COUNT] #define r_raw_rc_values (&r_page_raw_rc_input[PX4IO_P_RAW_RC_BASE]) #define r_rc_valid r_page_rc_input[PX4IO_P_RC_VALID] -#define r_rc_values (&r_page_rc_input[PX4IO_P_RAW_RC_BASE]) +#define r_rc_values (&r_page_rc_input[PX4IO_P_RC_BASE]) #define r_setup_features r_page_setup[PX4IO_P_SETUP_FEATURES] #define r_setup_arming r_page_setup[PX4IO_P_SETUP_ARMING] -- cgit v1.2.3 From b06d199129d57eabe2b73da713c9ac4ce98a68bf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 25 Jan 2014 23:10:48 +0100 Subject: Fixed year in controls.c comment. --- src/modules/px4iofirmware/controls.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 5e2c92bf4..ed37cc43d 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.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 -- cgit v1.2.3 From eee2508644ea1ca3b267ed89db6d0deb8fe0d3e1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jan 2014 00:14:19 +0100 Subject: Add additional flags to RC topic, not used yet. --- src/drivers/drv_rc_input.h | 28 ++++++++++++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h index 6b87141e9..0afe2f16f 100644 --- a/src/drivers/drv_rc_input.h +++ b/src/drivers/drv_rc_input.h @@ -92,6 +92,34 @@ struct rc_input_values { /** receive signal strength indicator (RSSI): < 0: Undefined, 0: no signal, 255: full reception */ int32_t rssi; + /** + * explicit failsafe flag: true on TX failure or TX out of range , false otherwise. + * Only the true state is reliable, as there are some (PPM) receivers on the market going + * into failsafe without telling us explicitly. + * */ + bool rc_failsafe; + + /** + * RC receiver connection status: True,if no frame has arrived in the expected time, false otherwise. + * True usally means that the receiver has been disconnected, but can also indicate a radio link loss on "stupid" systems. + * Will remain false, if a RX with failsafe option continues to transmit frames after a link loss. + * */ + bool rc_lost; + + /** + * Number of lost RC frames. + * Note: intended purpose: observe the radio link quality if RSSI is not available + * This value must not be used to trigger any failsafe-alike funtionality. + * */ + uint16_t rc_lost_frame_count; + + /** + * Number of total RC frames. + * Note: intended purpose: observe the radio link quality if RSSI is not available + * This value must not be used to trigger any failsafe-alike funtionality. + * */ + uint16_t rc_total_frame_count; + /** Input source */ enum RC_INPUT_SOURCE input_source; -- cgit v1.2.3 From 57d38bc8cec1362308f632e74e99485f82a35501 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jan 2014 00:17:26 +0100 Subject: Clean up RC related metadata, put everything into the RC data page. This ensures atomic reads, makes the reads more efficient and allows for some headroom for more RC flags. The IO driver side is updated as well, however, these flags are not published yet. --- src/drivers/px4io/px4io.cpp | 10 ++++--- src/modules/px4iofirmware/controls.c | 49 ++++++++++++++++++++++++++++------- src/modules/px4iofirmware/protocol.h | 14 +++++++--- src/modules/px4iofirmware/px4io.h | 3 ++- src/modules/px4iofirmware/registers.c | 8 ++++-- src/modules/px4iofirmware/sbus.c | 21 ++++++++------- 6 files changed, 76 insertions(+), 29 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 4bdf530a7..382acb6d0 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1763,6 +1763,7 @@ PX4IO::print_status() printf("%u bytes free\n", io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM)); uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS); + uint16_t io_status_flags = flags; printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s%s\n", flags, ((flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED) ? " OUTPUTS_ARMED" : ""), @@ -1770,8 +1771,6 @@ PX4IO::print_status() ((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""), ((flags & PX4IO_P_STATUS_FLAGS_RC_OK) ? " RC_OK" : " RC_FAIL"), ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM) ? " PPM" : ""), - (((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_STATUS_FLAGS_RC_DSM11))) ? " DSM10" : ""), - (((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_STATUS_FLAGS_RC_DSM11)) ? " DSM11" : ""), ((flags & PX4IO_P_STATUS_FLAGS_RC_SBUS) ? " SBUS" : ""), ((flags & PX4IO_P_STATUS_FLAGS_FMU_OK) ? " FMU_OK" : " FMU_FAIL"), ((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ? " RAW_PWM_PASSTHROUGH" : ""), @@ -1824,6 +1823,11 @@ PX4IO::print_status() printf("\n"); uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT); printf("%d raw R/C inputs", raw_inputs); + flags = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_FLAGS); + printf("status 0x%04x%s", flags, + (((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11))) ? " DSM10" : ""), + (((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11)) ? " DSM11" : "") + ); for (unsigned i = 0; i < raw_inputs; i++) printf(" %u", io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + i)); @@ -1831,7 +1835,7 @@ PX4IO::print_status() printf("\n"); if ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM)) { - int frame_len = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_RC_DATA); + int frame_len = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_DATA); printf("RC data (PPM frame len) %u us\n", frame_len); if ((frame_len - raw_inputs * 2000 - 3000) < 0) { diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index ed37cc43d..1cac977d7 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -97,28 +97,57 @@ controls_tick() { /* receive signal strenght indicator (RSSI). 0 = no connection, 255: perfect connection */ uint16_t rssi = 0; +#ifdef ADC_RSSI + unsigned counts = adc_measure(ADC_RSSI); + if (counts != 0xffff) { + /* use 1:1 scaling on 3.3V ADC input */ + unsigned mV = counts * 3300 / 4096; + + /* scale to 0..253 */ + rssi = mV / 13; + } +#endif + perf_begin(c_gather_dsm); uint16_t temp_count = r_raw_rc_count; bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count); if (dsm_updated) { - r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM; + r_raw_rc_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM; r_raw_rc_count = temp_count & 0x7fff; if (temp_count & 0x8000) - r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM11; + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11; else - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RC_DSM11; + r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11; - rssi = 255; } perf_end(c_gather_dsm); perf_begin(c_gather_sbus); - bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &rssi, PX4IO_RC_INPUT_CHANNELS); bool sbus_status = (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS); + bool sbus_failsafe, sbus_frame_drop; + bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, PX4IO_RC_INPUT_CHANNELS); + if (sbus_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS; + + rssi = 255; + + if (sbus_frame_drop) { + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP; + rssi = 100; + } else { + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); + } + + if (sbus_failsafe) { + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE; + rssi = 0; + } else { + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); + } + } /* switch S.Bus output pin as needed */ @@ -136,12 +165,9 @@ controls_tick() { * disable the PPM decoder completely if we have S.bus signal. */ perf_begin(c_gather_ppm); - bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_status[PX4IO_P_STATUS_RC_DATA]); + bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_raw_rc_input[PX4IO_P_RAW_RC_DATA]); if (ppm_updated) { - /* XXX sample RSSI properly here */ - rssi = 255; - r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM; } perf_end(c_gather_ppm); @@ -150,6 +176,9 @@ controls_tick() { if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS) r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS; + /* store RSSI */ + r_page_raw_rc_input[PX4IO_P_RAW_RC_NRSSI] = rssi; + /* * In some cases we may have received a frame, but input has still * been lost. @@ -247,7 +276,7 @@ controls_tick() { * This might happen if a protocol-based receiver returns an update * that contains no channels that we have mapped. */ - if (assigned_channels == 0 || rssi == 0) { + if (assigned_channels == 0 || (r_raw_rc_flags & (PX4IO_P_RAW_RC_FLAGS_FAILSAFE))) { rc_input_lost = true; } else { /* set RC OK flag */ diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index e5bef6eb3..738dc7d6e 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -111,7 +111,6 @@ #define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */ #define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */ #define PX4IO_P_STATUS_FLAGS_SAFETY_OFF (1 << 12) /* safety is off */ -#define PX4IO_P_STATUS_FLAGS_RC_DSM11 (1 << 13) /* DSM input is 11 bit data */ #define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */ #define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* [1] VBatt is very close to regulator dropout */ @@ -128,8 +127,6 @@ #define PX4IO_P_STATUS_VSERVO 6 /* [2] servo rail voltage in mV */ #define PX4IO_P_STATUS_VRSSI 7 /* [2] RSSI voltage */ #define PX4IO_P_STATUS_PRSSI 8 /* [2] RSSI PWM value */ -#define PX4IO_P_STATUS_NRSSI 9 /* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */ -#define PX4IO_P_STATUS_RC_DATA 10 /* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */ /* array of post-mix actuator outputs, -10000..10000 */ #define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */ @@ -140,7 +137,16 @@ /* array of raw RC input values, microseconds */ #define PX4IO_PAGE_RAW_RC_INPUT 4 #define PX4IO_P_RAW_RC_COUNT 0 /* number of valid channels */ -#define PX4IO_P_RAW_RC_BASE 1 /* CONFIG_RC_INPUT_COUNT channels from here */ +#define PX4IO_P_RAW_RC_FLAGS 1 /* RC detail status flags */ +#define PX4IO_P_RAW_RC_FLAGS_FRAME_DROP (1 << 0) /* single frame drop */ +#define PX4IO_P_RAW_RC_FLAGS_FAILSAFE (1 << 1) /* receiver is in failsafe mode */ +#define PX4IO_P_RAW_RC_FLAGS_RC_DSM11 (1 << 2) /* DSM decoding is 11 bit mode */ + +#define PX4IO_P_RAW_RC_NRSSI 2 /* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */ +#define PX4IO_P_RAW_RC_DATA 3 /* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */ +#define PX4IO_P_RAW_FRAME_COUNT 4 /* Number of total received frames (wrapping counter) */ +#define PX4IO_P_RAW_LOST_FRAME_COUNT 5 /* Number of total dropped frames (wrapping counter) */ +#define PX4IO_P_RAW_RC_BASE 6 /* CONFIG_RC_INPUT_COUNT channels from here */ /* array of scaled RC input values, -10000..10000 */ #define PX4IO_PAGE_RC_INPUT 5 diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index ee9d5b0c4..a2e1ed6cd 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -96,6 +96,7 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */ #define r_raw_rc_count r_page_raw_rc_input[PX4IO_P_RAW_RC_COUNT] #define r_raw_rc_values (&r_page_raw_rc_input[PX4IO_P_RAW_RC_BASE]) +#define r_raw_rc_flags r_page_raw_rc_input[PX4IO_P_RAW_RC_FLAGS] #define r_rc_valid r_page_rc_input[PX4IO_P_RC_VALID] #define r_rc_values (&r_page_rc_input[PX4IO_P_RC_BASE]) @@ -215,7 +216,7 @@ extern int dsm_init(const char *device); extern bool dsm_input(uint16_t *values, uint16_t *num_values); extern void dsm_bind(uint16_t cmd, int pulses); extern int sbus_init(const char *device); -extern bool sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels); +extern bool sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels); /** global debug level for isr_debug() */ extern volatile uint8_t debug_level; diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 2c437d2c0..477efa082 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -90,8 +90,6 @@ uint16_t r_page_status[] = { [PX4IO_P_STATUS_VSERVO] = 0, [PX4IO_P_STATUS_VRSSI] = 0, [PX4IO_P_STATUS_PRSSI] = 0, - [PX4IO_P_STATUS_NRSSI] = 0, - [PX4IO_P_STATUS_RC_DATA] = 0 }; /** @@ -116,6 +114,12 @@ uint16_t r_page_servos[PX4IO_SERVO_COUNT]; uint16_t r_page_raw_rc_input[] = { [PX4IO_P_RAW_RC_COUNT] = 0, + [PX4IO_P_RAW_RC_FLAGS] = 0, + [PX4IO_P_RAW_RC_NRSSI] = 0, + [PX4IO_P_RAW_RC_DATA] = 0, + [PX4IO_P_RAW_FRAME_COUNT] = 0, + [PX4IO_P_RAW_LOST_FRAME_COUNT] = 0, + [PX4IO_P_RAW_RC_DATA] = 0, [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_RC_INPUT_CHANNELS)] = 0 }; diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 495447740..32ab2a698 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -87,7 +87,7 @@ static unsigned partial_frame_count; unsigned sbus_frame_drops; -static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels); +static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels); int sbus_init(const char *device) @@ -118,7 +118,7 @@ sbus_init(const char *device) } bool -sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels) +sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels) { ssize_t ret; hrt_abstime now; @@ -175,7 +175,7 @@ sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_ * decode it. */ partial_frame_count = 0; - return sbus_decode(now, values, num_values, rssi, max_channels); + return sbus_decode(now, values, num_values, sbus_failsafe, sbus_frame_drop, max_channels); } /* @@ -215,7 +215,7 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = { }; static bool -sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_values) +sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values) { /* check frame boundary markers to avoid out-of-sync cases */ if ((frame[0] != 0x0f)) { @@ -289,20 +289,23 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint /* decode and handle failsafe and frame-lost flags */ if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FAILSAFE_BIT)) { /* failsafe */ /* report that we failed to read anything valid off the receiver */ - *rssi = 0; + *sbus_failsafe = true; + *sbus_frame_drop = true; return false; } else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */ - /* set a special warning flag or try to calculate some kind of RSSI information - to be implemented + /* set a special warning flag * * Attention! This flag indicates a skipped frame only, not a total link loss! Handling this * condition as fail-safe greatly reduces the reliability and range of the radio link, * e.g. by prematurely issueing return-to-launch!!! */ - *rssi = 100; // XXX magic number indicating bad signal, but not a signal loss (yet) + *sbus_failsafe = false; + *sbus_frame_drop = true; + } else { + *sbus_failsafe = false; + *sbus_frame_drop = false; } - *rssi = 255; - return true; } -- cgit v1.2.3 From a737a2a4061babb3de524ac2001a659786081e4a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jan 2014 13:22:26 +0100 Subject: RSSI and SBUS out config now handled as setup feature flags. --- src/modules/px4iofirmware/protocol.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 738dc7d6e..69c12d877 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -163,6 +163,10 @@ /* setup page */ #define PX4IO_PAGE_SETUP 50 #define PX4IO_P_SETUP_FEATURES 0 +#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT (1 << 0) /* enable S.Bus v1 output */ +#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT (1 << 1) /* enable S.Bus v2 output */ +#define PX4IO_P_SETUP_FEATURES_PWM_RSSI (1 << 2) /* enable PWM RSSI parsing */ +#define PX4IO_P_SETUP_FEATURES_ADC_RSSI (1 << 3) /* enable ADC RSSI parsing */ #define PX4IO_P_SETUP_ARMING 1 /* arming controls */ #define PX4IO_P_SETUP_ARMING_IO_ARM_OK (1 << 0) /* OK to arm the IO side */ -- cgit v1.2.3 From 16eb68f2e9b1de47b4f77d2ccaf72f102eb67fdf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jan 2014 14:13:57 +0100 Subject: Allow the setup flags to control the S.BUS / RSSI port instead of doing some wild, likely incorrect, guesses. --- src/modules/px4iofirmware/controls.c | 23 +++++++----------- src/modules/px4iofirmware/registers.c | 44 ++++++++++++++++++++++++++++++++--- 2 files changed, 50 insertions(+), 17 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 1cac977d7..e70eaed09 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -98,13 +98,15 @@ controls_tick() { uint16_t rssi = 0; #ifdef ADC_RSSI - unsigned counts = adc_measure(ADC_RSSI); - if (counts != 0xffff) { - /* use 1:1 scaling on 3.3V ADC input */ - unsigned mV = counts * 3300 / 4096; - - /* scale to 0..253 */ - rssi = mV / 13; + if (r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) { + unsigned counts = adc_measure(ADC_RSSI); + if (counts != 0xffff) { + /* use 1:1 scaling on 3.3V ADC input */ + unsigned mV = counts * 3300 / 4096; + + /* scale to 0..253 */ + rssi = mV / 13; + } } #endif @@ -150,13 +152,6 @@ controls_tick() { } - /* switch S.Bus output pin as needed */ - if (sbus_status != (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS)) { - #ifdef ENABLE_SBUS_OUT - ENABLE_SBUS_OUT((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS)); - #endif - } - perf_end(c_gather_sbus); /* diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 477efa082..2d7a1fe0d 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -148,7 +148,12 @@ uint16_t r_page_scratch[32]; */ volatile uint16_t r_page_setup[] = { +#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 + /* default to RSSI ADC functionality */ + [PX4IO_P_SETUP_FEATURES] = PX4IO_P_SETUP_FEATURES_ADC_RSSI, +#else [PX4IO_P_SETUP_FEATURES] = 0, +#endif [PX4IO_P_SETUP_ARMING] = 0, [PX4IO_P_SETUP_PWM_RATES] = 0, [PX4IO_P_SETUP_PWM_DEFAULTRATE] = 50, @@ -166,7 +171,14 @@ volatile uint16_t r_page_setup[] = [PX4IO_P_SETUP_CRC ... (PX4IO_P_SETUP_CRC+1)] = 0, }; -#define PX4IO_P_SETUP_FEATURES_VALID (0) +#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 +#define PX4IO_P_SETUP_FEATURES_VALID (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | \ + PX4IO_P_SETUP_FEATURES_SBUS2_OUT | \ + PX4IO_P_SETUP_FEATURES_ADC_RSSI | \ + PX4IO_P_SETUP_FEATURES_PWM_RSSI) +#else +#define PX4IO_P_SETUP_FEATURES_VALID 0 +#endif #define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_FMU_ARMED | \ PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \ PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \ @@ -442,9 +454,35 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_P_SETUP_FEATURES: value &= PX4IO_P_SETUP_FEATURES_VALID; - r_setup_features = value; - /* no implemented feature selection at this point */ + /* some of the options conflict - give S.BUS out precedence, then ADC RSSI, then PWM RSSI */ + + /* switch S.Bus output pin as needed */ + #ifdef ENABLE_SBUS_OUT + ENABLE_SBUS_OUT(value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT)); + + /* disable the conflicting options */ + if (value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT)) { + value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | PX4IO_P_SETUP_FEATURES_ADC_RSSI); + } + #endif + + /* disable the conflicting options with ADC RSSI */ + if (value & (PX4IO_P_SETUP_FEATURES_ADC_RSSI)) { + value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | + PX4IO_P_SETUP_FEATURES_SBUS1_OUT | + PX4IO_P_SETUP_FEATURES_SBUS2_OUT); + } + + /* disable the conflicting options with PWM RSSI (without effect here, but for completeness) */ + if (value & (PX4IO_P_SETUP_FEATURES_PWM_RSSI)) { + value &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI | + PX4IO_P_SETUP_FEATURES_SBUS1_OUT | + PX4IO_P_SETUP_FEATURES_SBUS2_OUT); + } + + /* apply changes */ + r_setup_features = value; break; -- cgit v1.2.3 From 731ab465b3d7d40ffb5ce3ca3d14660c6fee1ae6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jan 2014 14:22:54 +0100 Subject: Add support in the IO driver to control the S.Bus / RSSI port. --- src/drivers/drv_rc_input.h | 8 +++- src/drivers/drv_sbus.h | 58 +++++++++++++++++++++++++ src/drivers/px4io/px4io.cpp | 103 ++++++++++++++++++++++++++++++++++++++++++-- 3 files changed, 163 insertions(+), 6 deletions(-) create mode 100644 src/drivers/drv_sbus.h diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h index 0afe2f16f..715df7e01 100644 --- a/src/drivers/drv_rc_input.h +++ b/src/drivers/drv_rc_input.h @@ -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 @@ -135,8 +135,12 @@ ORB_DECLARE(input_rc); #define _RC_INPUT_BASE 0x2b00 /** Fetch R/C input values into (rc_input_values *)arg */ - #define RC_INPUT_GET _IOC(_RC_INPUT_BASE, 0) +/** Enable RSSI input via ADC */ +#define RC_INPUT_ENABLE_RSSI_ANALOG _IOC(_RC_INPUT_BASE, 1) + +/** Enable RSSI input via PWM signal */ +#define RC_INPUT_ENABLE_RSSI_PWM _IOC(_RC_INPUT_BASE, 2) #endif /* _DRV_RC_INPUT_H */ diff --git a/src/drivers/drv_sbus.h b/src/drivers/drv_sbus.h new file mode 100644 index 000000000..927c904ec --- /dev/null +++ b/src/drivers/drv_sbus.h @@ -0,0 +1,58 @@ +/**************************************************************************** + * + * Copyright (c) 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 drv_sbus.h + * + * Futaba S.BUS / S.BUS 2 compatible interface. + */ + +#ifndef _DRV_SBUS_H +#define _DRV_SBUS_H + +#include +#include + +#include "drv_orb_dev.h" + +/** + * Path for the default S.BUS device + */ +#define SBUS_DEVICE_PATH "/dev/sbus" + +#define _SBUS_BASE 0x2c00 + +/** Enable S.BUS version 1 / 2 output (0 to disable) */ +#define SBUS_SET_PROTO_VERSION _IOC(_SBUS_BASE, 0) + +#endif /* _DRV_SBUS_H */ diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 382acb6d0..0fb7b7d24 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -61,6 +61,7 @@ #include #include #include +#include #include #include #include @@ -1824,9 +1825,11 @@ PX4IO::print_status() uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT); printf("%d raw R/C inputs", raw_inputs); flags = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_FLAGS); - printf("status 0x%04x%s", flags, + printf("status 0x%04x%s%s%s%s", flags, (((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11))) ? " DSM10" : ""), - (((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11)) ? " DSM11" : "") + (((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11)) ? " DSM11" : ""), + ((flags & PX4IO_P_RAW_RC_FLAGS_FRAME_DROP) ? " FRAME_DROP" : ""), + ((flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) ? " FAILSAFE" : "") ); for (unsigned i = 0; i < raw_inputs; i++) @@ -1861,7 +1864,13 @@ PX4IO::print_status() printf("\n"); /* setup and state */ - printf("features 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES)); + uint16_t features = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES); + printf("features 0x%04x%s%s\n", features, + ((features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) ? " S.BUS1_OUT" : ""), + ((features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) ? " S.BUS2_OUT" : ""), + ((features & PX4IO_P_SETUP_FEATURES_PWM_RSSI) ? " RSSI_PWM" : ""), + ((features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) ? " RSSI_ADC" : "") + ); uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING); printf("arming 0x%04x%s%s%s%s%s%s\n", arming, @@ -2283,6 +2292,38 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) break; + case RC_INPUT_ENABLE_RSSI_ANALOG: + + if (arg) { + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_ADC_RSSI); + } else { + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, PX4IO_P_SETUP_FEATURES_ADC_RSSI, 0); + } + + break; + + case RC_INPUT_ENABLE_RSSI_PWM: + + if (arg) { + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_PWM_RSSI); + } else { + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, PX4IO_P_SETUP_FEATURES_PWM_RSSI, 0); + } + + break; + + case SBUS_SET_PROTO_VERSION: + + if (arg == 1) { + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS1_OUT); + } else if (arg == 2) { + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS2_OUT); + } else { + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT), 0); + } + + break; + default: /* not a recognized value */ ret = -ENOTTY; @@ -2932,6 +2973,60 @@ px4io_main(int argc, char *argv[]) if (!strcmp(argv[1], "bind")) bind(argc, argv); + if (!strcmp(argv[1], "sbus1_out")) { + /* we can cheat and call the driver directly, as it + * doesn't reference filp in ioctl() + */ + int ret = g_dev->ioctl(nullptr, SBUS_SET_PROTO_VERSION, 1); + + if (ret != 0) { + errx(ret, "S.BUS v1 failed"); + } + + exit(0); + } + + if (!strcmp(argv[1], "sbus2_out")) { + /* we can cheat and call the driver directly, as it + * doesn't reference filp in ioctl() + */ + int ret = g_dev->ioctl(nullptr, SBUS_SET_PROTO_VERSION, 2); + + if (ret != 0) { + errx(ret, "S.BUS v2 failed"); + } + + exit(0); + } + + if (!strcmp(argv[1], "rssi_analog")) { + /* we can cheat and call the driver directly, as it + * doesn't reference filp in ioctl() + */ + int ret = g_dev->ioctl(nullptr, RC_INPUT_ENABLE_RSSI_ANALOG, 1); + + if (ret != 0) { + errx(ret, "RSSI analog failed"); + } + + exit(0); + } + + if (!strcmp(argv[1], "rssi_pwm")) { + /* we can cheat and call the driver directly, as it + * doesn't reference filp in ioctl() + */ + int ret = g_dev->ioctl(nullptr, RC_INPUT_ENABLE_RSSI_PWM, 1); + + if (ret != 0) { + errx(ret, "RSSI PWM failed"); + } + + exit(0); + } + out: - errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'bind', 'checkcrc', 'forceupdate' or 'update'"); + errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug ',\n" + "'recovery', 'limit ', 'current', 'bind', 'checkcrc',\n" + "'forceupdate', 'update', 'sbus1_out', 'sbus2_out', 'rssi_analog' or 'rssi_pwm'"); } -- cgit v1.2.3 From 9cdbbab855d463bffb39d8dd55888fc1e0423818 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jan 2014 14:52:46 +0100 Subject: Differentiate between publication and signal receive timestamp, correctly set the rc_lost flag in the frame. Ready for prime-time testing. --- src/drivers/drv_rc_input.h | 18 ++++++++++++++++-- src/drivers/px4fmu/fmu.cpp | 12 ++++++++++-- src/drivers/px4io/px4io.cpp | 31 +++++++++++++++++++++++++------ src/modules/mavlink/orb_listener.c | 2 +- src/modules/sensors/sensors.cpp | 9 ++++++--- src/systemcmds/tests/test_rc.c | 2 +- 6 files changed, 59 insertions(+), 15 deletions(-) diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h index 715df7e01..20763e265 100644 --- a/src/drivers/drv_rc_input.h +++ b/src/drivers/drv_rc_input.h @@ -62,6 +62,11 @@ */ #define RC_INPUT_MAX_CHANNELS 18 +/** + * Maximum RSSI value + */ +#define RC_INPUT_RSSI_MAX 255 + /** * Input signal type, value is a control position from zero to 100 * percent. @@ -83,8 +88,11 @@ enum RC_INPUT_SOURCE { * on the board involved. */ struct rc_input_values { - /** decoding time */ - uint64_t timestamp; + /** publication time */ + uint64_t timestamp_publication; + + /** last valid reception time */ + uint64_t timestamp_last_signal; /** number of channels actually being seen */ uint32_t channel_count; @@ -120,6 +128,12 @@ struct rc_input_values { * */ uint16_t rc_total_frame_count; + /** + * Length of a single PPM frame. + * Zero for non-PPM systems + */ + uint16_t rc_ppm_frame_length; + /** Input source */ enum RC_INPUT_SOURCE input_source; diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index c067d363b..0fbd84924 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -626,7 +626,7 @@ PX4FMU::task_main() #ifdef HRT_PPM_CHANNEL // see if we have new PPM input data - if (ppm_last_valid_decode != rc_in.timestamp) { + if (ppm_last_valid_decode != rc_in.timestamp_last_signal) { // we have a new PPM frame. Publish it. rc_in.channel_count = ppm_decoded_channels; @@ -638,7 +638,15 @@ PX4FMU::task_main() rc_in.values[i] = ppm_buffer[i]; } - rc_in.timestamp = ppm_last_valid_decode; + rc_in.timestamp_publication = ppm_last_valid_decode; + rc_in.timestamp_last_signal = ppm_last_valid_decode; + + rc_in.rc_ppm_frame_length = ppm_frame_length; + rc_in.rssi = RC_INPUT_RSSI_MAX; + rc_in.rc_failsafe = false; + rc_in.rc_lost = false; + rc_in.rc_lost_frame_count = 0; + rc_in.rc_total_frame_count = 0; /* lazily advertise on first publication */ if (to_input_rc == 0) { diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 0fb7b7d24..5601230a4 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -239,6 +239,7 @@ private: unsigned _update_interval; ///< Subscription interval limiting send rate bool _rc_handling_disabled; ///< If set, IO does not evaluate, but only forward the RC values unsigned _rc_chan_count; ///< Internal copy of the last seen number of RC channels + uint64_t _rc_last_valid; ///< last valid timestamp volatile int _task; ///< worker task id volatile bool _task_should_exit; ///< worker terminate flag @@ -468,6 +469,7 @@ PX4IO::PX4IO(device::Device *interface) : _update_interval(0), _rc_handling_disabled(false), _rc_chan_count(0), + _rc_last_valid(0), _task(-1), _task_should_exit(false), _mavlink_fd(-1), @@ -1398,7 +1400,8 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) * * This should be the common case (9 channel R/C control being a reasonable upper bound). */ - input_rc.timestamp = hrt_absolute_time(); + input_rc.timestamp_publication = hrt_absolute_time(); + ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, ®s[0], prolog + 9); if (ret != OK) @@ -1408,13 +1411,25 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) * Get the channel count any any extra channels. This is no more expensive than reading the * channel count once. */ - channel_count = regs[0]; + channel_count = regs[PX4IO_P_RAW_RC_COUNT]; if (channel_count != _rc_chan_count) perf_count(_perf_chan_count); _rc_chan_count = channel_count; + input_rc.rc_ppm_frame_length = regs[PX4IO_P_RAW_RC_DATA]; + input_rc.rssi = regs[PX4IO_P_RAW_RC_NRSSI]; + input_rc.rc_failsafe = (regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_FAILSAFE); + input_rc.rc_lost_frame_count = regs[PX4IO_P_RAW_LOST_FRAME_COUNT]; + input_rc.rc_total_frame_count = regs[PX4IO_P_RAW_FRAME_COUNT]; + + /* rc_lost has to be set before the call to this function */ + if (!input_rc.rc_lost && !input_rc.rc_failsafe) + _rc_last_valid = input_rc.timestamp_publication; + + input_rc.timestamp_last_signal = _rc_last_valid; + if (channel_count > 9) { ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + 9, ®s[prolog + 9], channel_count - 9); @@ -1431,13 +1446,12 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) int PX4IO::io_publish_raw_rc() { - /* if no raw RC, just don't publish */ - if (!(_status & PX4IO_P_STATUS_FLAGS_RC_OK)) - return OK; /* fetch values from IO */ rc_input_values rc_val; - rc_val.timestamp = hrt_absolute_time(); + + /* set the RC status flag ORDER MATTERS! */ + rc_val.rc_lost = !(_status & PX4IO_P_STATUS_FLAGS_RC_OK); int ret = io_get_raw_rc_input(rc_val); @@ -1456,6 +1470,11 @@ PX4IO::io_publish_raw_rc() } else { rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN; + + /* we do not know the RC input, only publish if RC OK flag is set */ + /* if no raw RC, just don't publish */ + if (!(_status & PX4IO_P_STATUS_FLAGS_RC_OK)) + return OK; } /* lazily advertise on first publication */ diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 92b1b45be..41c754405 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -355,7 +355,7 @@ l_input_rc(const struct listener *l) for (unsigned i = 0; (i * port_width) < (rc_raw.channel_count + port_width); i++) { /* Channels are sent in MAVLink main loop at a fixed interval */ mavlink_msg_rc_channels_raw_send(chan, - rc_raw.timestamp / 1000, + rc_raw.timestamp_publication / 1000, i, (rc_raw.channel_count > (i * port_width) + 0) ? rc_raw.values[(i * port_width) + 0] : UINT16_MAX, (rc_raw.channel_count > (i * port_width) + 1) ? rc_raw.values[(i * port_width) + 1] : UINT16_MAX, diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 23f20b0cb..df6cbb7b2 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1276,6 +1276,9 @@ Sensors::rc_poll() orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); + if (rc_input.rc_lost) + return; + struct manual_control_setpoint_s manual_control; struct actuator_controls_s actuator_group_3; @@ -1320,7 +1323,7 @@ Sensors::rc_poll() channel_limit = _rc_max_chan_count; /* we are accepting this message */ - _rc_last_valid = rc_input.timestamp; + _rc_last_valid = rc_input.timestamp_last_signal; /* Read out values from raw message */ for (unsigned int i = 0; i < channel_limit; i++) { @@ -1369,9 +1372,9 @@ Sensors::rc_poll() } _rc.chan_count = rc_input.channel_count; - _rc.timestamp = rc_input.timestamp; + _rc.timestamp = rc_input.timestamp_last_signal; - manual_control.timestamp = rc_input.timestamp; + manual_control.timestamp = rc_input.timestamp_last_signal; /* roll input - rolling right is stick-wise and rotation-wise positive */ manual_control.roll = limit_minus_one_to_one(_rc.chan[_rc.function[ROLL]].scaled); diff --git a/src/systemcmds/tests/test_rc.c b/src/systemcmds/tests/test_rc.c index 6a602ecfc..57c0e7f4c 100644 --- a/src/systemcmds/tests/test_rc.c +++ b/src/systemcmds/tests/test_rc.c @@ -121,7 +121,7 @@ int test_rc(int argc, char *argv[]) return ERROR; } - if (hrt_absolute_time() - rc_input.timestamp > 100000) { + if (hrt_absolute_time() - rc_input.timestamp_last_signal > 100000) { warnx("TIMEOUT, less than 10 Hz updates"); (void)close(_rc_sub); return ERROR; -- cgit v1.2.3 From 2dc3cf5e43154a516505d768885e734a5ab25e14 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jan 2014 14:58:21 +0100 Subject: Remove unneeded header and commented out dead code from MEAS airspeed driver --- src/drivers/meas_airspeed/meas_airspeed.cpp | 22 +++++++--------------- 1 file changed, 7 insertions(+), 15 deletions(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index a95c4576b..9251cff7b 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -77,7 +77,6 @@ #include #include #include -#include #include #include @@ -178,24 +177,17 @@ MEASAirspeed::collect() return ret; } - //uint16_t diff_pres_pa = (val[1]) | ((val[0] & ~(0xC0)) << 8); - uint16_t temp = (val[3] & 0xE0) << 8 | val[2]; - - // XXX leaving this in until new calculation method has been cross-checked - //diff_pres_pa = abs(diff_pres_pa - (16384 / 2.0f)); - //diff_pres_pa -= _diff_pres_offset; int16_t dp_raw = 0, dT_raw = 0; dp_raw = (val[0] << 8) + val[1]; - dp_raw = 0x3FFF & dp_raw; //mask the used bits + /* mask the used bits */ + dp_raw = 0x3FFF & dp_raw; dT_raw = (val[2] << 8) + val[3]; dT_raw = (0xFFE0 & dT_raw) >> 5; float temperature = ((200 * dT_raw) / 2047) - 50; - // XXX we may want to smooth out the readings to remove noise. - - // Calculate differential pressure. As its centered around 8000 - // and can go positive or negative, enforce absolute value -// uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f)); + /* calculate differential pressure. As its centered around 8000 + * and can go positive or negative, enforce absolute value + */ const float P_min = -1.0f; const float P_max = 1.0f; float diff_press_pa = fabsf( ( ((float)dp_raw - 0.1f*16383.0f) * (P_max-P_min)/(0.8f*16383.0f) + P_min) * 6894.8f) - _diff_pres_offset; @@ -204,7 +196,7 @@ MEASAirspeed::collect() struct differential_pressure_s report; - // Track maximum differential pressure measured (so we can work out top speed). + /* track maximum differential pressure measured (so we can work out top speed). */ if (diff_press_pa > _max_differential_pressure_pa) { _max_differential_pressure_pa = diff_press_pa; } @@ -392,7 +384,7 @@ test() err(1, "immediate read failed"); warnx("single read"); - warnx("diff pressure: %d pa", report.differential_pressure_pa); + warnx("diff pressure: %d pa", (double)report.differential_pressure_pa); /* start the sensor polling at 2Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) -- cgit v1.2.3 From b67f7b2c7cb4d176ff8b3c5b7ee5e839845eef94 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jan 2014 15:12:12 +0100 Subject: Fix printing of IO status --- src/drivers/px4io/px4io.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 5601230a4..e24236de7 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1784,7 +1784,7 @@ PX4IO::print_status() io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM)); uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS); uint16_t io_status_flags = flags; - printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s%s\n", + printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s\n", flags, ((flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED) ? " OUTPUTS_ARMED" : ""), ((flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ? " SAFETY_OFF" : " SAFETY_SAFE"), @@ -1843,6 +1843,12 @@ PX4IO::print_status() printf("\n"); uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT); printf("%d raw R/C inputs", raw_inputs); + + for (unsigned i = 0; i < raw_inputs; i++) + printf(" %u", io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + i)); + + printf("\n"); + flags = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_FLAGS); printf("status 0x%04x%s%s%s%s", flags, (((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11))) ? " DSM10" : ""), @@ -1851,11 +1857,6 @@ PX4IO::print_status() ((flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) ? " FAILSAFE" : "") ); - for (unsigned i = 0; i < raw_inputs; i++) - printf(" %u", io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + i)); - - printf("\n"); - if ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM)) { int frame_len = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_DATA); printf("RC data (PPM frame len) %u us\n", frame_len); @@ -1884,7 +1885,7 @@ PX4IO::print_status() /* setup and state */ uint16_t features = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES); - printf("features 0x%04x%s%s\n", features, + printf("features 0x%04x%s%s%s%s\n", features, ((features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) ? " S.BUS1_OUT" : ""), ((features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) ? " S.BUS2_OUT" : ""), ((features & PX4IO_P_SETUP_FEATURES_PWM_RSSI) ? " RSSI_PWM" : ""), -- cgit v1.2.3 From ac32116f00a07d61b2873dd98a2a4ed28515d522 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jan 2014 15:17:01 +0100 Subject: Fix docs header --- src/modules/px4iofirmware/px4io.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index a2e1ed6cd..39272104d 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -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 -- cgit v1.2.3 From cc68d11353e7fd7f0e775b8c11d99bd21982d2a3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jan 2014 15:26:19 +0100 Subject: Add flag to indicate proper RC mapping --- src/modules/px4iofirmware/protocol.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 69c12d877..e3f499435 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -141,6 +141,7 @@ #define PX4IO_P_RAW_RC_FLAGS_FRAME_DROP (1 << 0) /* single frame drop */ #define PX4IO_P_RAW_RC_FLAGS_FAILSAFE (1 << 1) /* receiver is in failsafe mode */ #define PX4IO_P_RAW_RC_FLAGS_RC_DSM11 (1 << 2) /* DSM decoding is 11 bit mode */ +#define PX4IO_P_RAW_RC_FLAGS_MAPPING_OK (1 << 3) /* Channel mapping is ok */ #define PX4IO_P_RAW_RC_NRSSI 2 /* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */ #define PX4IO_P_RAW_RC_DATA 3 /* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */ -- cgit v1.2.3 From d1c934233f42e32a59117cbec317c1116d39be82 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jan 2014 15:30:16 +0100 Subject: Fix S.BUS decoder to return value even if its just failsafe --- src/modules/px4iofirmware/sbus.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 32ab2a698..f6ec542eb 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -291,7 +291,6 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool /* report that we failed to read anything valid off the receiver */ *sbus_failsafe = true; *sbus_frame_drop = true; - return false; } else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */ /* set a special warning flag -- cgit v1.2.3 From 2a30c574ce569c876dd2b95919a0d86c7c1b1023 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jan 2014 15:50:19 +0100 Subject: IO status printing improvements / fixes --- src/drivers/px4io/px4io.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index e24236de7..9595518ec 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1850,14 +1850,15 @@ PX4IO::print_status() printf("\n"); flags = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_FLAGS); - printf("status 0x%04x%s%s%s%s", flags, + printf("R/C flags: 0x%04x%s%s%s%s%s\n", flags, (((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11))) ? " DSM10" : ""), (((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11)) ? " DSM11" : ""), ((flags & PX4IO_P_RAW_RC_FLAGS_FRAME_DROP) ? " FRAME_DROP" : ""), - ((flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) ? " FAILSAFE" : "") + ((flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) ? " FAILSAFE" : ""), + ((flags & PX4IO_P_RAW_RC_FLAGS_MAPPING_OK) ? " MAPPING_OK" : "") ); - if ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM)) { + if ((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_PPM)) { int frame_len = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_DATA); printf("RC data (PPM frame len) %u us\n", frame_len); @@ -2717,7 +2718,7 @@ monitor(void) /* clear screen */ printf("\033[2J"); - unsigned cancels = 3; + unsigned cancels = 2; for (;;) { pollfd fds[1]; -- cgit v1.2.3 From 00a3270dc696e09ad1e8f7b0eec579b92b6c0e2e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jan 2014 15:52:51 +0100 Subject: Differentiate between failsafe having kicked in (which stops the normal output mixing based on RC outputs and prevents unwanted control commands due to failsafe) and a true loss of the receiver, where we stop outputting RC channel readings downstream on FMU. --- src/modules/px4iofirmware/controls.c | 187 +++++++++++++++++++---------------- src/modules/px4iofirmware/px4io.h | 3 +- 2 files changed, 106 insertions(+), 84 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index e70eaed09..b3a999f22 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -59,6 +59,11 @@ static perf_counter_t c_gather_ppm; void controls_init(void) { + /* no channels */ + r_raw_rc_count = 0; + rc_channels_timestamp_received = 0; + rc_channels_timestamp_valid = 0; + /* DSM input (USART1) */ dsm_init("/dev/ttyS0"); @@ -121,6 +126,9 @@ controls_tick() { else r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11; + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); + } perf_end(c_gather_dsm); @@ -164,6 +172,8 @@ controls_tick() { if (ppm_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM; + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); } perf_end(c_gather_ppm); @@ -185,97 +195,100 @@ controls_tick() { */ if (dsm_updated || sbus_updated || ppm_updated) { - /* update RC-received timestamp */ - system_state.rc_channels_timestamp = hrt_absolute_time(); - /* record a bitmask of channels assigned */ unsigned assigned_channels = 0; - /* map raw inputs to mapped inputs */ - /* XXX mapping should be atomic relative to protocol */ - for (unsigned i = 0; i < r_raw_rc_count; i++) { - - /* map the input channel */ - uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE]; - - if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { - - uint16_t raw = r_raw_rc_values[i]; - - int16_t scaled; - - /* - * 1) Constrain to min/max values, as later processing depends on bounds. - */ - if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) - raw = conf[PX4IO_P_RC_CONFIG_MIN]; - if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) - raw = conf[PX4IO_P_RC_CONFIG_MAX]; - - /* - * 2) Scale around the mid point differently for lower and upper range. - * - * This is necessary as they don't share the same endpoints and slope. - * - * First normalize to 0..1 range with correct sign (below or above center), - * then scale to 20000 range (if center is an actual center, -10000..10000, - * if parameters only support half range, scale to 10000 range, e.g. if - * center == min 0..10000, if center == max -10000..0). - * - * As the min and max bounds were enforced in step 1), division by zero - * cannot occur, as for the case of center == min or center == max the if - * statement is mutually exclusive with the arithmetic NaN case. - * - * DO NOT REMOVE OR ALTER STEP 1! - */ - if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { - scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])); - - } else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { - scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN])); - - } else { - /* in the configured dead zone, output zero */ - scaled = 0; - } - - /* invert channel if requested */ - if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) - scaled = -scaled; + /* update RC-received timestamp */ + system_state.rc_channels_timestamp_received = hrt_absolute_time(); + + /* do not command anything in failsafe, kick in the RC loss counter */ + if (!(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) { + + /* update RC-received timestamp */ + system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received; + + /* map raw inputs to mapped inputs */ + /* XXX mapping should be atomic relative to protocol */ + for (unsigned i = 0; i < r_raw_rc_count; i++) { + + /* map the input channel */ + uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE]; + + if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { + + uint16_t raw = r_raw_rc_values[i]; + + int16_t scaled; + + /* + * 1) Constrain to min/max values, as later processing depends on bounds. + */ + if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) + raw = conf[PX4IO_P_RC_CONFIG_MIN]; + if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) + raw = conf[PX4IO_P_RC_CONFIG_MAX]; + + /* + * 2) Scale around the mid point differently for lower and upper range. + * + * This is necessary as they don't share the same endpoints and slope. + * + * First normalize to 0..1 range with correct sign (below or above center), + * then scale to 20000 range (if center is an actual center, -10000..10000, + * if parameters only support half range, scale to 10000 range, e.g. if + * center == min 0..10000, if center == max -10000..0). + * + * As the min and max bounds were enforced in step 1), division by zero + * cannot occur, as for the case of center == min or center == max the if + * statement is mutually exclusive with the arithmetic NaN case. + * + * DO NOT REMOVE OR ALTER STEP 1! + */ + if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { + scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])); + + } else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { + scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN])); + + } else { + /* in the configured dead zone, output zero */ + scaled = 0; + } + + /* invert channel if requested */ + if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) + scaled = -scaled; - /* and update the scaled/mapped version */ - unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]; - if (mapped < PX4IO_CONTROL_CHANNELS) { + /* and update the scaled/mapped version */ + unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]; + if (mapped < PX4IO_CONTROL_CHANNELS) { - /* invert channel if pitch - pulling the lever down means pitching up by convention */ - if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */ - scaled = -scaled; + /* invert channel if pitch - pulling the lever down means pitching up by convention */ + if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */ + scaled = -scaled; - r_rc_values[mapped] = SIGNED_TO_REG(scaled); - assigned_channels |= (1 << mapped); + r_rc_values[mapped] = SIGNED_TO_REG(scaled); + assigned_channels |= (1 << mapped); + } } } - } - /* set un-assigned controls to zero */ - for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) { - if (!(assigned_channels & (1 << i))) - r_rc_values[i] = 0; - } + /* set un-assigned controls to zero */ + for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) { + if (!(assigned_channels & (1 << i))) + r_rc_values[i] = 0; + } - /* - * If we got an update with zero channels, treat it as - * a loss of input. - * - * This might happen if a protocol-based receiver returns an update - * that contains no channels that we have mapped. - */ - if (assigned_channels == 0 || (r_raw_rc_flags & (PX4IO_P_RAW_RC_FLAGS_FAILSAFE))) { - rc_input_lost = true; - } else { - /* set RC OK flag */ + /* set RC OK flag, as we got an update */ r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK; + + /* if we have enough channels (5) to control the vehicle, the mapping is ok */ + if (assigned_channels > 4) { + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK; + } else { + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK); + } } /* @@ -288,7 +301,7 @@ controls_tick() { * If we haven't seen any new control data in 200ms, assume we * have lost input. */ - if (hrt_elapsed_time(&system_state.rc_channels_timestamp) > 200000) { + if (hrt_elapsed_time(&system_state.rc_channels_timestamp_received) > 200000) { rc_input_lost = true; /* clear the input-kind flags here */ @@ -296,24 +309,32 @@ controls_tick() { PX4IO_P_STATUS_FLAGS_RC_PPM | PX4IO_P_STATUS_FLAGS_RC_DSM | PX4IO_P_STATUS_FLAGS_RC_SBUS); + } /* * Handle losing RC input */ - if (rc_input_lost) { + /* this kicks in if the receiver is gone or the system went to failsafe */ + if (rc_input_lost || (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) { /* Clear the RC input status flag, clear manual override flag */ r_status_flags &= ~( PX4IO_P_STATUS_FLAGS_OVERRIDE | PX4IO_P_STATUS_FLAGS_RC_OK); + /* Mark all channels as invalid, as we just lost the RX */ + r_rc_valid = 0; + /* Set the RC_LOST alarm */ r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST; + } - /* Mark the arrays as empty */ + /* this kicks in if the receiver is completely gone */ + if (rc_input_lost) { + + /* Set channel count to zero */ r_raw_rc_count = 0; - r_rc_valid = 0; } /* diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 39272104d..bb224f388 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -116,7 +116,8 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */ */ struct sys_state_s { - volatile uint64_t rc_channels_timestamp; + volatile uint64_t rc_channels_timestamp_received; + volatile uint64_t rc_channels_timestamp_valid; /** * Last FMU receive time, in microseconds since system boot -- cgit v1.2.3 From 0393b2aa129050307896b87c657ee0ed8e449891 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jan 2014 16:01:39 +0100 Subject: Build fix for IO control input parsing. --- src/modules/px4iofirmware/controls.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index b3a999f22..941500f0d 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -61,8 +61,8 @@ controls_init(void) { /* no channels */ r_raw_rc_count = 0; - rc_channels_timestamp_received = 0; - rc_channels_timestamp_valid = 0; + system_state.rc_channels_timestamp_received = 0; + system_state.rc_channels_timestamp_valid = 0; /* DSM input (USART1) */ dsm_init("/dev/ttyS0"); -- cgit v1.2.3 From ba90dc87f6fcb6198e36265239774d35a8efe9cd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 27 Jan 2014 16:43:44 +0100 Subject: HOTFIX: Re-enable legacy config support, uncomment commented out configs. Needs more work. --- ROMFS/px4fmu_common/init.d/rc.autostart | 40 ++++++++++++++++----------------- 1 file changed, 20 insertions(+), 20 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 325520dd0..75cac3e50 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -33,7 +33,7 @@ fi if param compare SYS_AUTOSTART 1002 then - #sh /etc/init.d/1002_rc_fw_state.hil + sh /etc/init.d/1002_rc_fw_state.hil fi if param compare SYS_AUTOSTART 1003 @@ -52,47 +52,47 @@ fi if param compare SYS_AUTOSTART 2100 100 then - #sh /etc/init.d/2100_mpx_easystar - #set MODE custom + sh /etc/init.d/2100_mpx_easystar + set MODE custom fi if param compare SYS_AUTOSTART 2101 101 then - #sh /etc/init.d/2101_hk_bixler - #set MODE custom + sh /etc/init.d/2101_hk_bixler + set MODE custom fi if param compare SYS_AUTOSTART 2102 102 then - #sh /etc/init.d/2102_3dr_skywalker - #set MODE custom + sh /etc/init.d/2102_3dr_skywalker + set MODE custom fi # # Flying wing # -if param compare SYS_AUTOSTART 3030 +if param compare SYS_AUTOSTART 3030 30 then - #sh /etc/init.d/3030_io_camflyer + sh /etc/init.d/3030_io_camflyer fi -if param compare SYS_AUTOSTART 3031 +if param compare SYS_AUTOSTART 3031 31 then sh /etc/init.d/3031_phantom fi -if param compare SYS_AUTOSTART 3032 +if param compare SYS_AUTOSTART 3032 32 then sh /etc/init.d/3032_skywalker_x5 fi -if param compare SYS_AUTOSTART 3033 +if param compare SYS_AUTOSTART 3033 33 then sh /etc/init.d/3033_wingwing fi -if param compare SYS_AUTOSTART 3034 +if param compare SYS_AUTOSTART 3034 34 then sh /etc/init.d/3034_fx79 fi @@ -101,27 +101,27 @@ fi # Quad X # -if param compare SYS_AUTOSTART 4008 +if param compare SYS_AUTOSTART 4008 8 then #sh /etc/init.d/4008_ardrone fi -if param compare SYS_AUTOSTART 4009 +if param compare SYS_AUTOSTART 4009 9 then #sh /etc/init.d/4009_ardrone_flow fi -if param compare SYS_AUTOSTART 4010 +if param compare SYS_AUTOSTART 4010 10 then sh /etc/init.d/4010_dji_f330 fi -if param compare SYS_AUTOSTART 4011 +if param compare SYS_AUTOSTART 4011 11 then sh /etc/init.d/4011_dji_f450 fi -if param compare SYS_AUTOSTART 4012 +if param compare SYS_AUTOSTART 4012 12 then sh /etc/init.d/4012_hk_x550 fi @@ -130,12 +130,12 @@ fi # Wide arm / H frame # -if param compare SYS_AUTOSTART 10015 +if param compare SYS_AUTOSTART 10015 15 then sh /etc/init.d/10015_tbs_discovery fi -if param compare SYS_AUTOSTART 10016 +if param compare SYS_AUTOSTART 10016 16 then sh /etc/init.d/10016_3dr_iris fi -- cgit v1.2.3 From 068cc190e20b8901b8e4128f3c21200724f0cd4f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 27 Jan 2014 17:14:18 +0100 Subject: Updated / added all system types that have been available before --- ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil | 58 +++------------ ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil | 56 +++------------ .../px4fmu_common/init.d/1004_rc_fw_Rascal110.hil | 1 - ROMFS/px4fmu_common/init.d/12001_octo_cox_pwm | 37 ++++++++++ ROMFS/px4fmu_common/init.d/2101_hk_bixler | 48 ++----------- ROMFS/px4fmu_common/init.d/2102_3dr_skywalker | 48 +------------ ROMFS/px4fmu_common/init.d/3030_io_camflyer | 84 +++++++++------------- ROMFS/px4fmu_common/init.d/3032_skywalker_x5 | 33 ++++++++- ROMFS/px4fmu_common/init.d/5001_quad_+_pwm | 37 ++++++++++ ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm | 37 ++++++++++ ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm | 37 ++++++++++ ROMFS/px4fmu_common/init.d/8001_octo_x_pwm | 37 ++++++++++ ROMFS/px4fmu_common/init.d/9001_octo_+_pwm | 37 ++++++++++ 13 files changed, 312 insertions(+), 238 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/12001_octo_cox_pwm create mode 100644 ROMFS/px4fmu_common/init.d/5001_quad_+_pwm create mode 100644 ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm create mode 100644 ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm create mode 100644 ROMFS/px4fmu_common/init.d/8001_octo_x_pwm create mode 100644 ROMFS/px4fmu_common/init.d/9001_octo_+_pwm diff --git a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil index 40a13b5d1..ebe8a1a1e 100644 --- a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil +++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil @@ -1,14 +1,13 @@ #!nsh # -# USB HIL start +# HILStar / X-Plane +# +# Maintainers: Thomas Gubler # -echo "[HIL] HILStar starting.." +echo "HIL Rascal 110 starting.." -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then # Set all params here, then disable autoconfig @@ -40,48 +39,7 @@ then param save fi -# Allow USB some time to come up -sleep 1 -# Tell MAVLink that this link is "fast" -mavlink start -b 230400 -d /dev/ttyACM0 - -# Create a fake HIL /dev/pwm_output interface -hil mode_pwm - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ -# -param set MAV_TYPE 1 - -# -# Check if we got an IO -# -if px4io start -then - echo "IO started" -else - fmu mode_serial - echo "FMU started" -fi - -# -# Start the sensors (depends on orb, px4io) -# -sh /etc/init.d/rc.sensors - -# -# Start the attitude estimator (depends on orb) -# -att_pos_estimator_ekf start - -# -# Load mixer and start controllers (depends on px4io) -# -mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix -fw_pos_control_l1 start -fw_att_control start - -echo "[HIL] setup done, running" +set HIL yes +set VEHICLE_TYPE fw +set MIXER FMU_AERT diff --git a/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil b/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil index 7b9f41bf6..46da24d35 100644 --- a/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil +++ b/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil @@ -1,14 +1,13 @@ #!nsh # -# USB HIL start +# HIL Rascal 110 (Flightgear) +# +# Maintainers: Thomas Gubler # -echo "[HIL] HILStar starting in state-HIL mode.." +echo "HIL Rascal 110 starting.." -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then # Set all params here, then disable autoconfig @@ -32,48 +31,15 @@ then param set FW_T_SINK_MAX 5.0 param set FW_T_SINK_MIN 4.0 param set FW_Y_ROLLFF 1.1 + param set FW_L1_PERIOD 16 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 param set SYS_AUTOCONFIG 0 param save fi -# Allow USB some time to come up -sleep 1 -# Tell MAVLink that this link is "fast" -mavlink start -b 230400 -d /dev/ttyACM0 - -# Create a fake HIL /dev/pwm_output interface -hil mode_pwm - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ -# -param set MAV_TYPE 1 - -# -# Check if we got an IO -# -if px4io start -then - echo "IO started" -else - fmu mode_serial - echo "FMU started" -fi - -# -# Start the sensors (depends on orb, px4io) -# -sh /etc/init.d/rc.sensors - -# -# Load mixer and start controllers (depends on px4io) -# -mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix -fw_pos_control_l1 start -fw_att_control start - -echo "[HIL] setup done, running" +set HIL yes +set VEHICLE_TYPE fw +set MIXER FMU_AERT diff --git a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil index 75a00a675..46da24d35 100644 --- a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil +++ b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil @@ -43,4 +43,3 @@ set HIL yes set VEHICLE_TYPE fw set MIXER FMU_AERT - diff --git a/ROMFS/px4fmu_common/init.d/12001_octo_cox_pwm b/ROMFS/px4fmu_common/init.d/12001_octo_cox_pwm new file mode 100644 index 000000000..5f3cec4e0 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/12001_octo_cox_pwm @@ -0,0 +1,37 @@ +#!nsh +# +# Generic 10” Octo coaxial geometry +# +# Maintainers: Lorenz Meier +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_YAWPOS_P 2.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + # TODO add default MPC parameters +fi + +set VEHICLE_TYPE mc +set MIXER FMU_octo_cox + +set PWM_OUTPUTS 1234 +set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 +set PWM_MIN 1200 +set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/2101_hk_bixler b/ROMFS/px4fmu_common/init.d/2101_hk_bixler index 995d3ba07..1ed923b19 100644 --- a/ROMFS/px4fmu_common/init.d/2101_hk_bixler +++ b/ROMFS/px4fmu_common/init.d/2101_hk_bixler @@ -1,11 +1,11 @@ #!nsh -echo "[init] PX4FMU v1, v2 with or without IO on HK Bixler" +echo "[init] PX4FMU v1, v2 with or without IO on 3DR SkyWalker" # # Load default params for this platform # -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then # Set all params here, then disable autoconfig param set FW_P_D 0 @@ -35,46 +35,6 @@ then param set SYS_AUTOCONFIG 0 param save fi - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - - sh /etc/init.d/rc.io - # Limit to 100 Hz updates and (implicit) 50 Hz PWM - px4io limit 100 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_AERT.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_AERT.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_AERT.mix -else - echo "Using /etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix -fi -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing +set VEHICLE_TYPE fw +set MIXER FMU_AERT \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker b/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker index a6d2ace96..1ed923b19 100644 --- a/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker +++ b/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker @@ -5,7 +5,7 @@ echo "[init] PX4FMU v1, v2 with or without IO on 3DR SkyWalker" # # Load default params for this platform # -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then # Set all params here, then disable autoconfig param set FW_P_D 0 @@ -35,48 +35,6 @@ then param set SYS_AUTOCONFIG 0 param save fi - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - - sh /etc/init.d/rc.io - # Limit to 100 Hz updates and (implicit) 50 Hz PWM - px4io limit 100 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi -pwm disarmed -c 3 -p 1056 - -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_AERT.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_AERT.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_AETR.mix -else - echo "Using /etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_AETR.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing +set VEHICLE_TYPE fw +set MIXER FMU_AERT \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/3030_io_camflyer b/ROMFS/px4fmu_common/init.d/3030_io_camflyer index 65f01c974..cbcc6189b 100644 --- a/ROMFS/px4fmu_common/init.d/3030_io_camflyer +++ b/ROMFS/px4fmu_common/init.d/3030_io_camflyer @@ -2,57 +2,39 @@ echo "[init] PX4FMU v1, v2 with or without IO on Camflyer" -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then - # Set all params here, then disable autoconfig - # TODO - - param set SYS_AUTOCONFIG 0 - param save + # + # Default parameters for this platform + # + param set FW_AIRSPD_MIN 7 + param set FW_AIRSPD_TRIM 9 + param set FW_AIRSPD_MAX 14 + param set FW_L1_PERIOD 10 + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 20 + param set FW_P_LIM_MAX 30 + param set FW_P_LIM_MIN -20 + param set FW_P_P 30 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 2 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 20 + param set FW_R_P 60 + param set FW_R_RMAX 60 + param set FW_THR_CRUISE 0.65 + param set FW_THR_MAX 0.7 + param set FW_THR_MIN 0 + param set FW_T_SINK_MAX 5 + param set FW_T_SINK_MIN 2 + param set FW_T_TIME_CONST 9 + param set FW_Y_ROLLFF 2.0 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 fi - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - - sh /etc/init.d/rc.io - # Limit to 100 Hz updates and (implicit) 50 Hz PWM - px4io limit 100 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix -else - echo "Using /etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing +set VEHICLE_TYPE fw +set MIXER FMU_Q diff --git a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 index 03f282237..143310af9 100644 --- a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 +++ b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 @@ -7,8 +7,37 @@ if [ $DO_AUTOCONFIG == yes ] then - # TODO + # + # Default parameters for this platform + # + param set FW_AIRSPD_MIN 7 + param set FW_AIRSPD_TRIM 9 + param set FW_AIRSPD_MAX 14 + param set FW_L1_PERIOD 10 + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 20 + param set FW_P_LIM_MAX 30 + param set FW_P_LIM_MIN -20 + param set FW_P_P 30 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 2 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 20 + param set FW_R_P 60 + param set FW_R_RMAX 60 + param set FW_THR_CRUISE 0.65 + param set FW_THR_MAX 0.7 + param set FW_THR_MIN 0 + param set FW_T_SINK_MAX 5 + param set FW_T_SINK_MIN 2 + param set FW_T_TIME_CONST 9 + param set FW_Y_ROLLFF 2.0 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 fi set VEHICLE_TYPE fw -set MIXER FMU_Q +set MIXER FMU_X5 diff --git a/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm b/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm new file mode 100644 index 000000000..2e5f6ca4f --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm @@ -0,0 +1,37 @@ +#!nsh +# +# Generic 10” Quad + geometry +# +# Maintainers: Lorenz Meier +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_YAWPOS_P 2.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + # TODO add default MPC parameters +fi + +set VEHICLE_TYPE mc +set MIXER FMU_quad_+ + +set PWM_OUTPUTS 1234 +set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 +set PWM_MIN 1200 +set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm b/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm new file mode 100644 index 000000000..ddec8f36e --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm @@ -0,0 +1,37 @@ +#!nsh +# +# Generic 10” Hexa X geometry +# +# Maintainers: Lorenz Meier +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_YAWPOS_P 2.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + # TODO add default MPC parameters +fi + +set VEHICLE_TYPE mc +set MIXER FMU_hexa_x + +set PWM_OUTPUTS 1234 +set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 +set PWM_MIN 1200 +set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm b/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm new file mode 100644 index 000000000..106e0fb54 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm @@ -0,0 +1,37 @@ +#!nsh +# +# Generic 10” Hexa + geometry +# +# Maintainers: Lorenz Meier +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_YAWPOS_P 2.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + # TODO add default MPC parameters +fi + +set VEHICLE_TYPE mc +set MIXER FMU_hexa_+ + +set PWM_OUTPUTS 1234 +set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 +set PWM_MIN 1200 +set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm b/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm new file mode 100644 index 000000000..f0eea339b --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm @@ -0,0 +1,37 @@ +#!nsh +# +# Generic 10” Octo X geometry +# +# Maintainers: Lorenz Meier +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_YAWPOS_P 2.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + # TODO add default MPC parameters +fi + +set VEHICLE_TYPE mc +set MIXER FMU_octo_x + +set PWM_OUTPUTS 1234 +set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 +set PWM_MIN 1200 +set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm b/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm new file mode 100644 index 000000000..992a7aeba --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm @@ -0,0 +1,37 @@ +#!nsh +# +# Generic 10” Octo + geometry +# +# Maintainers: Lorenz Meier +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_YAWPOS_P 2.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + # TODO add default MPC parameters +fi + +set VEHICLE_TYPE mc +set MIXER FMU_octo_+ + +set PWM_OUTPUTS 1234 +set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 +set PWM_MIN 1200 +set PWM_MAX 1900 -- cgit v1.2.3 From 4c4cd41b72471ce28ccebc4b24b592d6159f8626 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 27 Jan 2014 17:14:31 +0100 Subject: Registered all new system types --- ROMFS/px4fmu_common/init.d/rc.autostart | 54 +++++++++++++++++++++++++++++++++ 1 file changed, 54 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 75cac3e50..34da2dfef 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -126,6 +126,51 @@ then sh /etc/init.d/4012_hk_x550 fi +# +# Quad + +# + +if param compare SYS_AUTOSTART 5001 +then + sh /etc/init.d/5001_quad_+_pwm +fi + +# +# Hexa X +# + +if param compare SYS_AUTOSTART 6001 +then + sh /etc/init.d/6001_hexa_x_pwm +fi + +# +# Hexa + +# + +if param compare SYS_AUTOSTART 7001 +then + sh /etc/init.d/7001_hexa_+_pwm +fi + +# +# Octo X +# + +if param compare SYS_AUTOSTART 8001 +then + sh /etc/init.d/8001_octo_x_pwm +fi + +# +# Octo + +# + +if param compare SYS_AUTOSTART 9001 +then + sh /etc/init.d/9001_octo_+_pwm +fi + # # Wide arm / H frame # @@ -139,3 +184,12 @@ if param compare SYS_AUTOSTART 10016 16 then sh /etc/init.d/10016_3dr_iris fi + +# +# Octo Coaxial +# + +if param compare SYS_AUTOSTART 12001 +then + sh /etc/init.d/12001_octo_cox_pwm +fi -- cgit v1.2.3 From 7e855b5a23c9053dc8a59fcf33670e1cb93dd34b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 27 Jan 2014 17:20:24 +0100 Subject: Deleted test - should not be in mainline --- ROMFS/px4fmu_common/init.d/cmp_test | 9 --------- 1 file changed, 9 deletions(-) delete mode 100644 ROMFS/px4fmu_common/init.d/cmp_test diff --git a/ROMFS/px4fmu_common/init.d/cmp_test b/ROMFS/px4fmu_common/init.d/cmp_test deleted file mode 100644 index f86f4f85b..000000000 --- a/ROMFS/px4fmu_common/init.d/cmp_test +++ /dev/null @@ -1,9 +0,0 @@ -#!nsh - -cp /etc/extras/px4io-v2_default.bin /fs/microsd/px4io.loaded -if cmp /etc/extras/px4io-v2_default.bin /fs/microsd/px4io.loaded -then - echo "CMP returned true" -else - echo "CMP returned false" -fi \ No newline at end of file -- cgit v1.2.3 From dfaa5a0c7c81f432d64cfffdaaf3d683c01f10b4 Mon Sep 17 00:00:00 2001 From: Darryl Taylor Date: Tue, 28 Jan 2014 09:57:47 +0800 Subject: Reduced stack from 2048 to 1024. Top reports stack usage at 812 under flight conditions. --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index df847a64d..dbe97a178 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -756,7 +756,7 @@ PX4IO::init() } /* start the IO interface task */ - _task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 2048, (main_t)&PX4IO::task_main_trampoline, nullptr); + _task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 1024, (main_t)&PX4IO::task_main_trampoline, nullptr); if (_task < 0) { debug("task start failed: %d", errno); -- cgit v1.2.3 From d1fb7651876236432dc66c5331c60258ff962352 Mon Sep 17 00:00:00 2001 From: Darryl Taylor Date: Tue, 28 Jan 2014 10:30:16 +0800 Subject: Reduced low priority thread stack size to 1728. Top indicates the high-water-mark is at 1380 during accelerometer calibration. Safety margin ~25% --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index add7312de..6f15e0702 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -648,7 +648,7 @@ int commander_thread_main(int argc, char *argv[]) pthread_attr_t commander_low_prio_attr; pthread_attr_init(&commander_low_prio_attr); - pthread_attr_setstacksize(&commander_low_prio_attr, 2992); + pthread_attr_setstacksize(&commander_low_prio_attr, 1728); struct sched_param param; (void)pthread_attr_getschedparam(&commander_low_prio_attr, ¶m); -- cgit v1.2.3 From 0488d5b41c5c91658494242f778c7b9de99bcca4 Mon Sep 17 00:00:00 2001 From: Darryl Taylor Date: Tue, 28 Jan 2014 10:43:33 +0800 Subject: Reduced commander main task stack size to 2088. The high-water-mark measured at 1668 after calibration and flight. 25% safety margin, but commander is fairly complex. There are surely untested code paths here, but each is relatively shallow. --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 6f15e0702..33589940c 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -237,7 +237,7 @@ int commander_main(int argc, char *argv[]) daemon_task = task_spawn_cmd("commander", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, - 3000, + 2088, commander_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); -- cgit v1.2.3 From 19c3525f58b4123c38592644252226219ddef07a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 28 Jan 2014 11:11:34 +0100 Subject: Hotfix: Fixed telemetry transmission of RC channels - we always sent one set too much - by lieron --- src/modules/mavlink/orb_listener.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 92b1b45be..192c8fdba 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -352,7 +352,7 @@ l_input_rc(const struct listener *l) const unsigned port_width = 8; - for (unsigned i = 0; (i * port_width) < (rc_raw.channel_count + port_width); i++) { + for (unsigned i = 0; (i * port_width) < rc_raw.channel_count; i++) { /* Channels are sent in MAVLink main loop at a fixed interval */ mavlink_msg_rc_channels_raw_send(chan, rc_raw.timestamp / 1000, -- cgit v1.2.3 From 338b753a3c56813c5820e54209b4949e04a12ad9 Mon Sep 17 00:00:00 2001 From: Darryl Taylor Date: Wed, 29 Jan 2014 17:00:07 +0800 Subject: Reduced stack size to 1200. Max stack space used was 956. --- 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 20853379d..ebf01a2f4 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -773,7 +773,7 @@ int mavlink_main(int argc, char *argv[]) mavlink_task = task_spawn_cmd("mavlink", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 2048, + 1200, mavlink_thread_main, (const char **)argv); -- cgit v1.2.3 From 58a1f19d79da8a621454f11055534610921accd6 Mon Sep 17 00:00:00 2001 From: Darryl Taylor Date: Wed, 29 Jan 2014 17:09:05 +0800 Subject: Stack size reduced to 1816. Max stack reported by top was 1448 under HIL. --- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 7b6fad658..9fc7b748a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -871,7 +871,7 @@ receive_start(int uart) param.sched_priority = SCHED_PRIORITY_MAX - 40; (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); - pthread_attr_setstacksize(&receiveloop_attr, 3000); + pthread_attr_setstacksize(&receiveloop_attr, 1816); pthread_t thread; pthread_create(&thread, &receiveloop_attr, receive_thread, &uart); -- cgit v1.2.3 From b0f65bb708e2728d562dbef8db04a846170d0ca6 Mon Sep 17 00:00:00 2001 From: Darryl Taylor Date: Wed, 29 Jan 2014 17:12:16 +0800 Subject: Stack size reduced to 1648. Max stack usage reported top at 1316. --- src/modules/mavlink/orb_listener.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 92b1b45be..ed04882b9 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -838,7 +838,7 @@ uorb_receive_start(void) pthread_attr_init(&uorb_attr); /* Set stack size, needs less than 2k */ - pthread_attr_setstacksize(&uorb_attr, 2048); + pthread_attr_setstacksize(&uorb_attr, 1648); pthread_t thread; pthread_create(&thread, &uorb_attr, uorb_receive_thread, NULL); -- cgit v1.2.3 From 70b1037c2ebd38e06a3d12dca7bad295da02e16c Mon Sep 17 00:00:00 2001 From: Darryl Taylor Date: Wed, 29 Jan 2014 17:35:34 +0800 Subject: Stack size reduced to 2408. Max stack usage reported by top in EASY mode at 1924. --- src/modules/multirotor_pos_control/multirotor_pos_control.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 3d23d0c09..a89c7eace 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -126,7 +126,7 @@ int multirotor_pos_control_main(int argc, char *argv[]) deamon_task = task_spawn_cmd("multirotor_pos_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 60, - 4096, + 2408, multirotor_pos_control_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); -- cgit v1.2.3 From 0cc311b872c688f2242a4745cd3a6de4933c9e62 Mon Sep 17 00:00:00 2001 From: Darryl Taylor Date: Wed, 29 Jan 2014 17:39:35 +0800 Subject: Reduced stack size to 2568. Max stack usage reported as 2052 with MARG + GPS with 3d lock - no px4flow, but should be more than enough buffer to accomodate its addition. --- src/modules/position_estimator_inav/position_estimator_inav_main.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 3084b6d92..eb5a23b69 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -128,7 +128,7 @@ int position_estimator_inav_main(int argc, char *argv[]) thread_should_exit = false; position_estimator_inav_task = task_spawn_cmd("position_estimator_inav", - SCHED_RR, SCHED_PRIORITY_MAX - 5, 4096, + SCHED_RR, SCHED_PRIORITY_MAX - 5, 2568, position_estimator_inav_thread_main, (argv) ? (const char **) &argv[2] : (const char **) NULL); exit(0); -- cgit v1.2.3 From 1d70a65f40895b0c9e1ee5c65970efa1eae841e8 Mon Sep 17 00:00:00 2001 From: Darryl Taylor Date: Wed, 29 Jan 2014 17:52:22 +0800 Subject: Stack size reduced to 1280. Max stack size reported as 1020 with UBX GPS with 3d lock. MTK not tested. --- src/drivers/gps/gps.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 6b72d560f..f2faf711b 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -209,7 +209,7 @@ GPS::init() goto out; /* start the GPS driver worker task */ - _task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 2048, (main_t)&GPS::task_main_trampoline, nullptr); + _task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 1280, (main_t)&GPS::task_main_trampoline, nullptr); if (_task < 0) { warnx("task start failed: %d", errno); -- cgit v1.2.3 From 6b709e21d3aa5e88458b4f3319a39b4abbadc085 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 29 Jan 2014 21:26:28 +1100 Subject: FMUv2: set SPI GPIO pins on bus 1 to 50MHz this solves the ms5611 issue related to temperature. We now need to test if it is OK with all sensors --- nuttx-configs/px4fmu-v2/include/board.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/nuttx-configs/px4fmu-v2/include/board.h b/nuttx-configs/px4fmu-v2/include/board.h index 507df70a2..c6c771491 100755 --- a/nuttx-configs/px4fmu-v2/include/board.h +++ b/nuttx-configs/px4fmu-v2/include/board.h @@ -260,9 +260,9 @@ * * There are sensors on SPI1, and SPI2 is connected to the FRAM. */ -#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1 -#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1 -#define GPIO_SPI1_SCK GPIO_SPI1_SCK_1 +#define GPIO_SPI1_MISO (GPIO_SPI1_MISO_1|GPIO_SPEED_50MHz) +#define GPIO_SPI1_MOSI (GPIO_SPI1_MOSI_1|GPIO_SPEED_50MHz) +#define GPIO_SPI1_SCK (GPIO_SPI1_SCK_1|GPIO_SPEED_50MHz) #define GPIO_SPI2_MISO GPIO_SPI2_MISO_1 #define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_1 -- cgit v1.2.3 From e7e3e132adcbc755cd6cc2b97f6c8c827a9140bf Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 29 Jan 2014 21:30:15 +1100 Subject: FMUv2: push SPI2 GPIO speed up to 50MHz as well --- nuttx-configs/px4fmu-v2/include/board.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/nuttx-configs/px4fmu-v2/include/board.h b/nuttx-configs/px4fmu-v2/include/board.h index c6c771491..e56b14ba4 100755 --- a/nuttx-configs/px4fmu-v2/include/board.h +++ b/nuttx-configs/px4fmu-v2/include/board.h @@ -264,9 +264,9 @@ #define GPIO_SPI1_MOSI (GPIO_SPI1_MOSI_1|GPIO_SPEED_50MHz) #define GPIO_SPI1_SCK (GPIO_SPI1_SCK_1|GPIO_SPEED_50MHz) -#define GPIO_SPI2_MISO GPIO_SPI2_MISO_1 -#define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_1 -#define GPIO_SPI2_SCK GPIO_SPI2_SCK_2 +#define GPIO_SPI2_MISO (GPIO_SPI2_MISO_1|GPIO_SPEED_50MHz) +#define GPIO_SPI2_MOSI (GPIO_SPI2_MOSI_1|GPIO_SPEED_50MHz) +#define GPIO_SPI2_SCK (GPIO_SPI2_SCK_2|GPIO_SPEED_50MHz) /************************************************************************************ * Public Data -- cgit v1.2.3 From 08a6057ef8c4aa796751c5ac07ab8efa7529b150 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 29 Jan 2014 13:23:06 +0100 Subject: Increase SPI GPIO speed for FMUv1 analog to v2 --- nuttx-configs/px4fmu-v1/include/board.h | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/include/board.h b/nuttx-configs/px4fmu-v1/include/board.h index 27ace4b7d..ff1c63424 100644 --- a/nuttx-configs/px4fmu-v1/include/board.h +++ b/nuttx-configs/px4fmu-v1/include/board.h @@ -246,14 +246,14 @@ * * There are sensors on SPI1, and SPI3 is connected to the microSD slot. */ -#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1 -#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1 -#define GPIO_SPI1_SCK GPIO_SPI1_SCK_1 - -#define GPIO_SPI3_MISO GPIO_SPI3_MISO_2 -#define GPIO_SPI3_MOSI GPIO_SPI3_MOSI_1 -#define GPIO_SPI3_SCK GPIO_SPI3_SCK_2 -#define GPIO_SPI3_NSS GPIO_SPI3_NSS_2 +#define GPIO_SPI1_MISO (GPIO_SPI1_MISO_1|GPIO_SPEED_50MHz) +#define GPIO_SPI1_MOSI (GPIO_SPI1_MOSI_1|GPIO_SPEED_50MHz) +#define GPIO_SPI1_SCK (GPIO_SPI1_SCK_1|GPIO_SPEED_50MHz) + +#define GPIO_SPI3_MISO (GPIO_SPI3_MISO_2|GPIO_SPEED_50MHz) +#define GPIO_SPI3_MOSI (GPIO_SPI3_MOSI_1|GPIO_SPEED_50MHz) +#define GPIO_SPI3_SCK (GPIO_SPI3_SCK_2|GPIO_SPEED_50MHz) +#define GPIO_SPI3_NSS (GPIO_SPI3_NSS_2|GPIO_SPEED_50MHz) /* SPI DMA configuration for SPI3 (microSD) */ #define DMACHAN_SPI3_RX DMAMAP_SPI3_RX_1 -- cgit v1.2.3 From d40382425f3b79bd662949e7259479e568e8698e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 29 Jan 2014 14:51:57 +0100 Subject: Triggering param documentation generation --- Tools/px4params/.gitignore | 3 ++- Tools/px4params/dokuwikiout.py | 29 +++++++++++++++++++++++++++-- Tools/px4params/xmlrpc.sh | 5 +++++ 3 files changed, 34 insertions(+), 3 deletions(-) create mode 100644 Tools/px4params/xmlrpc.sh diff --git a/Tools/px4params/.gitignore b/Tools/px4params/.gitignore index 73cf39575..5d0378b4a 100644 --- a/Tools/px4params/.gitignore +++ b/Tools/px4params/.gitignore @@ -1,2 +1,3 @@ parameters.wiki -parameters.xml \ No newline at end of file +parameters.xml +cookies.txt \ No newline at end of file diff --git a/Tools/px4params/dokuwikiout.py b/Tools/px4params/dokuwikiout.py index 4d40a6201..c4fadbd35 100644 --- a/Tools/px4params/dokuwikiout.py +++ b/Tools/px4params/dokuwikiout.py @@ -1,10 +1,24 @@ import output +from xml.sax.saxutils import escape class DokuWikiOutput(output.Output): def Generate(self, groups): - result = "" + pre_text = """ + + wiki.putPage + + + + :dev:parameters:reference + + + + + """ + result = "====== Parameter Reference ======\nThis list is auto-generated every few minutes and contains the most recent parameter names and default values." for group in groups: result += "==== %s ====\n\n" % group.GetName() + result += "|< 100% 20% 20% 10% 10% 10% 30%>|\n" result += "^ Name ^ Description ^ Min ^ Max ^ Default ^ Comment ^\n" for param in group.GetParams(): code = param.GetFieldValue("code") @@ -34,4 +48,15 @@ class DokuWikiOutput(output.Output): result += "|" result += "|\n" result += "\n" - return result + post_text = """ + + + + + sum + Updated parameters automagically from code. + + + + """ + return pre_text + escape(result) + post_text diff --git a/Tools/px4params/xmlrpc.sh b/Tools/px4params/xmlrpc.sh new file mode 100644 index 000000000..36c52ff71 --- /dev/null +++ b/Tools/px4params/xmlrpc.sh @@ -0,0 +1,5 @@ +python px_process_params.py + +rm cookies.txt +curl --cookie cookies.txt --cookie-jar cookies.txt --user-agent Mozilla/4.0 --data "u=$XMLRPCUSER&p=$XMLRPCPASS" https://pixhawk.org/start?do=login +curl -k --cookie cookies.txt -H "Content-Type: application/xml" -X POST --data-binary @parameters.wiki "https://pixhawk.org/lib/exe/xmlrpc.php" -- cgit v1.2.3 From 60859607ff240f1aaad05b306ba86b84d15be2ac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 29 Jan 2014 15:11:50 +0100 Subject: Better output --- Tools/px4params/dokuwikiout.py | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/Tools/px4params/dokuwikiout.py b/Tools/px4params/dokuwikiout.py index c4fadbd35..42da19e8b 100644 --- a/Tools/px4params/dokuwikiout.py +++ b/Tools/px4params/dokuwikiout.py @@ -27,26 +27,26 @@ class DokuWikiOutput(output.Output): result += "| %s | %s " % (code, name) min_val = param.GetFieldValue("min") if min_val is not None: - result += "| %s " % min_val + result += " | %s " % min_val else: - result += "|" + result += " | " max_val = param.GetFieldValue("max") if max_val is not None: - result += "| %s " % max_val + result += " | %s " % max_val else: - result += "|" + result += " | " def_val = param.GetFieldValue("default") if def_val is not None: result += "| %s " % def_val else: - result += "|" + 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 += " | " + result += " |\n" result += "\n" post_text = """ -- cgit v1.2.3 From 2b17909f0c4131f6a728697f5de1eb13a1337234 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 29 Jan 2014 19:18:44 +0100 Subject: Changed param location --- Tools/px4params/dokuwikiout.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/px4params/dokuwikiout.py b/Tools/px4params/dokuwikiout.py index 42da19e8b..c5cf65ea6 100644 --- a/Tools/px4params/dokuwikiout.py +++ b/Tools/px4params/dokuwikiout.py @@ -9,7 +9,7 @@ class DokuWikiOutput(output.Output): - :dev:parameters:reference + :firmware:parameters -- cgit v1.2.3 From 1458bdfbcb34d251da2476386864ee680407b90f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 30 Jan 2014 13:07:17 +0100 Subject: Pure code style fix of cpuload, no funcationality changes --- src/modules/systemlib/cpuload.c | 3 +-- src/modules/systemlib/cpuload.h | 12 ++++++------ 2 files changed, 7 insertions(+), 8 deletions(-) diff --git a/src/modules/systemlib/cpuload.c b/src/modules/systemlib/cpuload.c index afc5b072c..ccc516f39 100644 --- a/src/modules/systemlib/cpuload.c +++ b/src/modules/systemlib/cpuload.c @@ -93,8 +93,7 @@ void cpuload_initialize_once() #endif /* CONFIG_SCHED_WORKQUEUE */ // perform static initialization of "system" threads - for (system_load.total_count = 0; system_load.total_count < static_tasks_count; system_load.total_count++) - { + for (system_load.total_count = 0; system_load.total_count < static_tasks_count; system_load.total_count++) { system_load.tasks[system_load.total_count].start_time = now; system_load.tasks[system_load.total_count].total_runtime = 0; system_load.tasks[system_load.total_count].curr_start_time = 0; diff --git a/src/modules/systemlib/cpuload.h b/src/modules/systemlib/cpuload.h index c7aa18d3c..16d132fdb 100644 --- a/src/modules/systemlib/cpuload.h +++ b/src/modules/systemlib/cpuload.h @@ -40,15 +40,15 @@ __BEGIN_DECLS #include struct system_load_taskinfo_s { - uint64_t total_runtime; ///< Runtime since start (start_time - total_runtime)/(start_time - current_time) = load - uint64_t curr_start_time; ///< Start time of the current scheduling slot - uint64_t start_time; ///< FIRST start time of task - FAR struct tcb_s *tcb; ///< - bool valid; ///< Task is currently active / valid + uint64_t total_runtime; ///< Runtime since start (start_time - total_runtime)/(start_time - current_time) = load + uint64_t curr_start_time; ///< Start time of the current scheduling slot + uint64_t start_time; ///< FIRST start time of task + FAR struct tcb_s *tcb; ///< + bool valid; ///< Task is currently active / valid }; struct system_load_s { - uint64_t start_time; ///< Global start time of measurements + uint64_t start_time; ///< Global start time of measurements struct system_load_taskinfo_s tasks[CONFIG_MAX_TASKS]; uint8_t initialized; int total_count; -- cgit v1.2.3 From eb177def141d321b43a4c20819179423e128a92f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 30 Jan 2014 19:44:06 +0100 Subject: mavlink: revert stack size 2048 to fix suspending in HIL mode --- 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 ebf01a2f4..20853379d 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -773,7 +773,7 @@ int mavlink_main(int argc, char *argv[]) mavlink_task = task_spawn_cmd("mavlink", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 1200, + 2048, mavlink_thread_main, (const char **)argv); -- cgit v1.2.3 From 5316741ed40965b837fab77074ff4fbd4fe6f858 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 30 Jan 2014 21:53:27 +0100 Subject: Revert "mavlink: revert stack size 2048 to fix suspending in HIL mode" This reverts commit eb177def141d321b43a4c20819179423e128a92f. --- 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 20853379d..ebf01a2f4 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -773,7 +773,7 @@ int mavlink_main(int argc, char *argv[]) mavlink_task = task_spawn_cmd("mavlink", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 2048, + 1200, mavlink_thread_main, (const char **)argv); -- cgit v1.2.3 From 8d79d919504b4b92ad05a7ebc12334083ae0f4b9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 30 Jan 2014 21:54:29 +0100 Subject: Revert "Merge pull request #620 from pigeonhunter/stack_sizes" This reverts commit 3b31a6b1b9756eb191eaaafb1c137e6874079281, reversing changes made to 70afb3ca3b3f1844241c9c9312579bbb2475232c. --- src/drivers/gps/gps.cpp | 2 +- src/drivers/px4io/px4io.cpp | 2 +- src/modules/commander/commander.cpp | 4 ++-- src/modules/mavlink/mavlink.c | 2 +- src/modules/mavlink/mavlink_receiver.cpp | 2 +- src/modules/mavlink/orb_listener.c | 2 +- src/modules/multirotor_pos_control/multirotor_pos_control.c | 2 +- src/modules/position_estimator_inav/position_estimator_inav_main.c | 2 +- 8 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index f2faf711b..6b72d560f 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -209,7 +209,7 @@ GPS::init() goto out; /* start the GPS driver worker task */ - _task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 1280, (main_t)&GPS::task_main_trampoline, nullptr); + _task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 2048, (main_t)&GPS::task_main_trampoline, nullptr); if (_task < 0) { warnx("task start failed: %d", errno); diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 5da288661..efcf4d12b 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -761,7 +761,7 @@ PX4IO::init() } /* start the IO interface task */ - _task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 1024, (main_t)&PX4IO::task_main_trampoline, nullptr); + _task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 2048, (main_t)&PX4IO::task_main_trampoline, nullptr); if (_task < 0) { debug("task start failed: %d", errno); diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 52cf25086..2a2bcca72 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -250,7 +250,7 @@ int commander_main(int argc, char *argv[]) daemon_task = task_spawn_cmd("commander", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, - 2088, + 3000, commander_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); @@ -685,7 +685,7 @@ int commander_thread_main(int argc, char *argv[]) pthread_attr_t commander_low_prio_attr; pthread_attr_init(&commander_low_prio_attr); - pthread_attr_setstacksize(&commander_low_prio_attr, 1728); + pthread_attr_setstacksize(&commander_low_prio_attr, 2992); struct sched_param param; (void)pthread_attr_getschedparam(&commander_low_prio_attr, ¶m); diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index ebf01a2f4..20853379d 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -773,7 +773,7 @@ int mavlink_main(int argc, char *argv[]) mavlink_task = task_spawn_cmd("mavlink", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 1200, + 2048, mavlink_thread_main, (const char **)argv); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 9fc7b748a..7b6fad658 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -871,7 +871,7 @@ receive_start(int uart) param.sched_priority = SCHED_PRIORITY_MAX - 40; (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); - pthread_attr_setstacksize(&receiveloop_attr, 1816); + pthread_attr_setstacksize(&receiveloop_attr, 3000); pthread_t thread; pthread_create(&thread, &receiveloop_attr, receive_thread, &uart); diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index bfb824db7..e1dabfd21 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -838,7 +838,7 @@ uorb_receive_start(void) pthread_attr_init(&uorb_attr); /* Set stack size, needs less than 2k */ - pthread_attr_setstacksize(&uorb_attr, 1648); + pthread_attr_setstacksize(&uorb_attr, 2048); pthread_t thread; pthread_create(&thread, &uorb_attr, uorb_receive_thread, NULL); diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index a89c7eace..3d23d0c09 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -126,7 +126,7 @@ int multirotor_pos_control_main(int argc, char *argv[]) deamon_task = task_spawn_cmd("multirotor_pos_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 60, - 2408, + 4096, multirotor_pos_control_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index eb5a23b69..3084b6d92 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -128,7 +128,7 @@ int position_estimator_inav_main(int argc, char *argv[]) thread_should_exit = false; position_estimator_inav_task = task_spawn_cmd("position_estimator_inav", - SCHED_RR, SCHED_PRIORITY_MAX - 5, 2568, + SCHED_RR, SCHED_PRIORITY_MAX - 5, 4096, position_estimator_inav_thread_main, (argv) ? (const char **) &argv[2] : (const char **) NULL); exit(0); -- cgit v1.2.3 From 83df116c7aa21b6d68f2aa31c4526dd822495d70 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 30 Jan 2014 23:11:37 +0100 Subject: Hotfix: Move mixer variables in test routine into function --- src/systemcmds/tests/test_mixer.cpp | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index 2a47551ee..df382e2c6 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -65,20 +65,20 @@ static int mixer_callback(uintptr_t handle, 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]; -uint16_t servo_predicted[output_max]; - -/* - * PWM limit structure - */ -pwm_limit_t pwm_limit; int test_mixer(int argc, char *argv[]) { + /* + * PWM limit structure + */ + pwm_limit_t pwm_limit; + 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]; + uint16_t servo_predicted[output_max]; + warnx("testing mixer"); char *filename = "/etc/mixers/IO_pass.mix"; -- cgit v1.2.3