From 291f4f3a33e6428b23624b1ffe12fec1015816cd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 19 Sep 2012 18:53:29 +0200 Subject: [PATCH 01/56] Reworked control interface, needs testing / validation --- apps/commander/commander.c | 25 +++++ apps/mavlink/mavlink.c | 103 ++++++++++++++---- .../multirotor_att_control_main.c | 31 ++++-- .../multirotor_attitude_control.c | 2 +- .../multirotor_rate_control.c | 8 +- .../multirotor_rate_control.h | 4 +- apps/px4/ground_estimator/ground_estimator.c | 2 +- apps/uORB/objects_common.cpp | 4 +- apps/uORB/topics/vehicle_attitude_setpoint.h | 17 ++- apps/uORB/topics/vehicle_command.h | 4 + ...rs_setpoint.h => vehicle_rates_setpoint.h} | 27 +++-- apps/uORB/topics/vehicle_status.h | 2 +- 12 files changed, 165 insertions(+), 64 deletions(-) rename apps/uORB/topics/{ardrone_motors_setpoint.h => vehicle_rates_setpoint.h} (70%) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index c8564792f8..f5e143066a 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -742,6 +742,31 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } break; + case PX4_CMD_CONTROLLER_SELECTION: { + bool changed = false; + if ((int)cmd->param1 != (int)current_vehicle_status->flag_control_rates_enabled) { + current_vehicle_status->flag_control_rates_enabled = cmd->param1; + changed = true; + } + if ((int)cmd->param2 != (int)current_vehicle_status->flag_control_attitude_enabled) { + current_vehicle_status->flag_control_attitude_enabled = cmd->param2; + changed = true; + } + if ((int)cmd->param3 != (int)current_vehicle_status->flag_control_velocity_enabled) { + current_vehicle_status->flag_control_velocity_enabled = cmd->param3; + changed = true; + } + if ((int)cmd->param4 != (int)current_vehicle_status->flag_control_position_enabled) { + current_vehicle_status->flag_control_position_enabled = cmd->param4; + changed = true; + } + + if (changed) { + /* publish current state machine */ + state_machine_publish(status_pub, current_vehicle_status, mavlink_fd); + } + } + // /* request to land */ // case MAV_CMD_NAV_LAND: // { diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index fb96866b0d..6470e65c89 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -65,7 +65,7 @@ #include #include #include -#include +#include #include #include #include @@ -126,14 +126,13 @@ static struct vehicle_attitude_s hil_attitude; static struct vehicle_global_position_s hil_global_pos; -static struct ardrone_motors_setpoint_s ardrone_motors; +static struct vehicle_rates_setpoint_s vehicle_rates_sp; static struct vehicle_command_s vcmd; static struct actuator_armed_s armed; static orb_advert_t pub_hil_global_pos = -1; -static orb_advert_t ardrone_motors_pub = -1; static orb_advert_t cmd_pub = -1; static orb_advert_t flow_pub = -1; static orb_advert_t global_position_setpoint_pub = -1; @@ -188,6 +187,12 @@ static struct mavlink_subscriptions { .initialized = false }; +static struct mavlink_publications { + orb_advert_t vehicle_rates_sp_pub; +} mavlink_pubs = { + .vehicle_rates_sp_pub = -1 +}; + /* 3: Define waypoint helper functions */ void mavlink_wpm_send_message(mavlink_message_t *msg); @@ -1133,6 +1138,13 @@ mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) } } +#define MAVLINK_OFFBOARD_CONTROL_MODE_NONE 0 +#define MAVLINK_OFFBOARD_CONTROL_MODE_RATES 1 +#define MAVLINK_OFFBOARD_CONTROL_MODE_ATTITUDE 2 +#define MAVLINK_OFFBOARD_CONTROL_MODE_VELOCITY 3 +#define MAVLINK_OFFBOARD_CONTROL_MODE_POSITION 4 +#define MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED 0x10 + /**************************************************************************** * Public Functions ****************************************************************************/ @@ -1242,29 +1254,80 @@ void handleMessage(mavlink_message_t *msg) // printf("got MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT target_system=%u, sysid = %u\n", quad_motors_setpoint.target_system, mavlink_system.sysid); if (mavlink_system.sysid < 4) { - ardrone_motors.p1 = quad_motors_setpoint.roll[mavlink_system.sysid]; - ardrone_motors.p2 = quad_motors_setpoint.pitch[mavlink_system.sysid]; - ardrone_motors.p3 = quad_motors_setpoint.yaw[mavlink_system.sysid]; - ardrone_motors.p4 = quad_motors_setpoint.thrust[mavlink_system.sysid]; - - ardrone_motors.timestamp = hrt_absolute_time(); - /* only send if RC is off */ if (v_status.rc_signal_lost) { - /* check if input has to be enabled */ - if (!v_status.flag_control_offboard_enabled) { - /* XXX Enable offboard control */ - } - /* XXX decode mode and set flags */ - // if (mode == abc) xxx flag_control_rates_enabled; + /* rate control mode */ + if (quad_motors_setpoint.mode == 1) { + vehicle_rates_sp.roll = quad_motors_setpoint.roll[mavlink_system.sysid] / (float)INT16_MAX; + vehicle_rates_sp.pitch = quad_motors_setpoint.pitch[mavlink_system.sysid] / (float)INT16_MAX; + vehicle_rates_sp.yaw = quad_motors_setpoint.yaw[mavlink_system.sysid] / (float)INT16_MAX; + vehicle_rates_sp.thrust = quad_motors_setpoint.thrust[mavlink_system.sysid] / (float)UINT16_MAX; + + vehicle_rates_sp.timestamp = hrt_absolute_time(); + } /* check if topic has to be advertised */ - if (ardrone_motors_pub <= 0) { - ardrone_motors_pub = orb_advertise(ORB_ID(ardrone_motors_setpoint), &ardrone_motors); + if (mavlink_pubs.vehicle_rates_sp_pub <= 0) { + mavlink_pubs.vehicle_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &vehicle_rates_sp); } /* Publish */ - orb_publish(ORB_ID(ardrone_motors_setpoint), ardrone_motors_pub, &ardrone_motors); + orb_publish(ORB_ID(vehicle_rates_setpoint), mavlink_pubs.vehicle_rates_sp_pub, &vehicle_rates_sp); + + /* change armed status if required */ + bool cmd_armed = (quad_motors_setpoint.mode & MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED); + + bool cmd_generated = false; + + if (v_status.flag_control_offboard_enabled != cmd_armed) { + vcmd.param1 = cmd_armed; + vcmd.param2 = 0; + vcmd.param3 = 0; + vcmd.param4 = 0; + vcmd.param5 = 0; + vcmd.param6 = 0; + vcmd.param7 = 0; + vcmd.command = MAV_CMD_COMPONENT_ARM_DISARM; + vcmd.target_system = mavlink_system.sysid; + vcmd.target_component = MAV_COMP_ID_ALL; + vcmd.source_system = msg->sysid; + vcmd.source_component = msg->compid; + vcmd.confirmation = 1; + + cmd_generated = true; + } + + /* check if input has to be enabled */ + if ((v_status.flag_control_rates_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_RATES)) || + (v_status.flag_control_attitude_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_ATTITUDE)) || + (v_status.flag_control_velocity_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_VELOCITY)) || + (v_status.flag_control_position_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_POSITION))) { + vcmd.param1 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_RATES); + vcmd.param2 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_ATTITUDE); + vcmd.param3 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_VELOCITY); + vcmd.param4 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_POSITION); + vcmd.param5 = 0; + vcmd.param6 = 0; + vcmd.param7 = 0; + vcmd.command = PX4_CMD_CONTROLLER_SELECTION; + vcmd.target_system = mavlink_system.sysid; + vcmd.target_component = MAV_COMP_ID_ALL; + vcmd.source_system = msg->sysid; + vcmd.source_component = msg->compid; + vcmd.confirmation = 1; + + cmd_generated = true; + } + + if (cmd_generated) { + /* check if topic is advertised */ + if (cmd_pub <= 0) { + cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); + } else { + /* create command */ + orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); + } + } } } } @@ -1479,7 +1542,7 @@ int mavlink_thread_main(int argc, char *argv[]) memset(&rc, 0, sizeof(rc)); memset(&hil_attitude, 0, sizeof(hil_attitude)); memset(&hil_global_pos, 0, sizeof(hil_global_pos)); - memset(&ardrone_motors, 0, sizeof(ardrone_motors)); + memset(&vehicle_rates_sp, 0, sizeof(vehicle_rates_sp)); memset(&vcmd, 0, sizeof(vcmd)); /* print welcome text */ diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 5300337015..3f0d562459 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -58,13 +58,14 @@ #include #include #include -#include +#include #include #include #include #include "multirotor_attitude_control.h" +#include "multirotor_rate_control.h" __EXPORT int multirotor_att_control_main(int argc, char *argv[]); @@ -93,18 +94,18 @@ mc_thread_main(int argc, char *argv[]) memset(&manual, 0, sizeof(manual)); struct sensor_combined_s raw; memset(&raw, 0, sizeof(raw)); - struct ardrone_motors_setpoint_s setpoint; - memset(&setpoint, 0, sizeof(setpoint)); + struct vehicle_rates_setpoint_s rates_sp; + memset(&rates_sp, 0, sizeof(rates_sp)); struct actuator_controls_s actuators; /* subscribe to attitude, motor setpoints and system state */ int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + int setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); int state_sub = orb_subscribe(ORB_ID(vehicle_status)); int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); - int setpoint_sub = orb_subscribe(ORB_ID(ardrone_motors_setpoint)); /* * Do not rate-limit the loop to prevent aliasing @@ -145,6 +146,12 @@ mc_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); /* get a local copy of attitude setpoint */ orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp); + /* get a local copy of rates setpoint */ + bool updated; + orb_check(setpoint_sub, &updated); + if (updated) { + orb_copy(ORB_ID(vehicle_rates_setpoint), setpoint_sub, &rates_sp); + } /* get a local copy of the current sensor values */ orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); @@ -155,16 +162,15 @@ mc_thread_main(int argc, char *argv[]) /* manual inputs, from RC control or joystick */ att_sp.roll_body = manual.roll; att_sp.pitch_body = manual.pitch; - att_sp.yaw_rate_body = manual.yaw; + att_sp.yaw_body = manual.yaw; // XXX Hack, remove, switch to yaw rate controller + /* set yaw rate */ + rates_sp.yaw = manual.yaw; att_sp.timestamp = hrt_absolute_time(); if (motor_test_mode) { att_sp.roll_body = 0.0f; att_sp.pitch_body = 0.0f; att_sp.yaw_body = 0.0f; - att_sp.roll_rate_body = 0.0f; - att_sp.pitch_rate_body = 0.0f; - att_sp.yaw_rate_body = 0.0f; att_sp.thrust = 0.1f; } else { att_sp.thrust = manual.throttle; @@ -184,7 +190,14 @@ mc_thread_main(int argc, char *argv[]) /* XXX could be also run in an independent loop */ if (state.flag_control_rates_enabled) { - multirotor_control_rates(&att_sp, &raw.gyro_rad_s, &actuators); + /* lowpass gyros */ + // XXX + static float gyro_lp[3] = {0, 0, 0}; + gyro_lp[0] = raw.gyro_rad_s[0]; + gyro_lp[1] = raw.gyro_rad_s[1]; + gyro_lp[2] = raw.gyro_rad_s[2]; + + multirotor_control_rates(&rates_sp, gyro_lp, &actuators); } /* STEP 3: publish the result to the vehicle actuators */ diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c index 997aa0c108..d7413913bd 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -243,7 +243,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s float roll_control = pid_calculate(&roll_controller, att_sp->roll_body + p.att_yoff, att->roll, att->rollspeed, deltaT); /* control yaw rate */ - float yaw_rate_control = pid_calculate(&yaw_speed_controller, att_sp->yaw_rate_body, att->yawspeed, 0.0f, deltaT); + float yaw_rate_control = pid_calculate(&yaw_speed_controller, att_sp->yaw_body, att->yawspeed, 0.0f, deltaT); /* * compensate the vertical loss of thrust diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c index 8a3ac78cdc..fefd1f7679 100644 --- a/apps/multirotor_att_control/multirotor_rate_control.c +++ b/apps/multirotor_att_control/multirotor_rate_control.c @@ -132,7 +132,7 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru return OK; } -void multirotor_control_rates(const struct vehicle_attitude_setpoint_s *rate_sp, +void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, const float rates[], struct actuator_controls_s *actuators) { static uint64_t last_run = 0; @@ -179,13 +179,13 @@ void multirotor_control_rates(const struct vehicle_attitude_setpoint_s *rate_sp, /* calculate current control outputs */ /* control pitch (forward) output */ - float pitch_control = pid_calculate(&pitch_controller, rate_sp->pitch_rate_body, + float pitch_control = pid_calculate(&pitch_controller, rate_sp->pitch, rates[1], 0.0f, deltaT); /* control roll (left/right) output */ - float roll_control = pid_calculate(&roll_controller, rate_sp->roll_rate_body, + float roll_control = pid_calculate(&roll_controller, rate_sp->roll, rates[0], 0.0f, deltaT); /* control yaw rate */ - float yaw_rate_control = pid_calculate(&yaw_speed_controller, rate_sp->yaw_rate_body, rates[2], 0.0f, deltaT); + float yaw_rate_control = pid_calculate(&yaw_speed_controller, rate_sp->yaw, rates[2], 0.0f, deltaT); /* * compensate the vertical loss of thrust diff --git a/apps/multirotor_att_control/multirotor_rate_control.h b/apps/multirotor_att_control/multirotor_rate_control.h index 6f34d99e7a..3c3c508014 100644 --- a/apps/multirotor_att_control/multirotor_rate_control.h +++ b/apps/multirotor_att_control/multirotor_rate_control.h @@ -47,10 +47,10 @@ #include #include -#include +#include #include -void multirotor_control_rates(const struct vehicle_attitude_setpoint_s *rate_sp, +void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, const float rates[], struct actuator_controls_s *actuators); #endif /* MULTIROTOR_RATE_CONTROL_H_ */ diff --git a/apps/px4/ground_estimator/ground_estimator.c b/apps/px4/ground_estimator/ground_estimator.c index 46f86a348d..07426ec2bd 100644 --- a/apps/px4/ground_estimator/ground_estimator.c +++ b/apps/px4/ground_estimator/ground_estimator.c @@ -75,7 +75,7 @@ int ground_estimator_thread_main(int argc, char *argv[]) { /* subscribe to raw data */ int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); - /* advertise attitude */ + /* advertise debug value */ struct debug_key_value_s dbg = { .key = "posx", .value = 0.0f }; orb_advert_t pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg); diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp index 1cad57a436..be85a345ae 100644 --- a/apps/uORB/objects_common.cpp +++ b/apps/uORB/objects_common.cpp @@ -78,8 +78,8 @@ ORB_DEFINE(vehicle_global_position, struct vehicle_global_position_s); #include "topics/vehicle_local_position.h" ORB_DEFINE(vehicle_local_position, struct vehicle_local_position_s); -#include "topics/ardrone_motors_setpoint.h" -ORB_DEFINE(ardrone_motors_setpoint, struct ardrone_motors_setpoint_s); +#include "topics/vehicle_rates_setpoint.h" +ORB_DEFINE(vehicle_rates_setpoint, struct vehicle_rates_setpoint_s); #include "topics/rc_channels.h" ORB_DEFINE(rc_channels, struct rc_channels_s); diff --git a/apps/uORB/topics/vehicle_attitude_setpoint.h b/apps/uORB/topics/vehicle_attitude_setpoint.h index 0c33ebff8c..62367cf77a 100644 --- a/apps/uORB/topics/vehicle_attitude_setpoint.h +++ b/apps/uORB/topics/vehicle_attitude_setpoint.h @@ -56,21 +56,18 @@ struct vehicle_attitude_setpoint_s { uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ - float roll_tait_bryan; /**< Tait-Bryan angle in NED frame */ - float pitch_tait_bryan; /**< Tait-Bryan angle in NED frame */ - float yaw_tait_bryan; /**< Tait-Bryan angle in NED frame */ - float tait_bryan_valid; /**< Set to true if Tait-Bryan angles are valid */ + //float roll_tait_bryan; /**< Tait-Bryan angle in NED frame */ + //float pitch_tait_bryan; /**< Tait-Bryan angle in NED frame */ + //float yaw_tait_bryan; /**< Tait-Bryan angle in NED frame */ + //float tait_bryan_valid; /**< Set to true if Tait-Bryan angles are valid */ float roll_body; /**< body angle in NED frame */ float pitch_body; /**< body angle in NED frame */ float yaw_body; /**< body angle in NED frame */ - float roll_rate_body; /**< body angle in NED frame */ - float pitch_rate_body; /**< body angle in NED frame */ - float yaw_rate_body; /**< body angle in NED frame */ - float body_valid; /**< Set to true if Tait-Bryan angles are valid */ + //float body_valid; /**< Set to true if body angles are valid */ - float R_body[9]; /**< Rotation matrix describing the setpoint as rotation from the current body frame */ - bool R_valid; /**< Set to true if rotation matrix is valid */ + //float R_body[9]; /**< Rotation matrix describing the setpoint as rotation from the current body frame */ + //bool R_valid; /**< Set to true if rotation matrix is valid */ float thrust; /**< Thrust in Newton the power system should generate */ diff --git a/apps/uORB/topics/vehicle_command.h b/apps/uORB/topics/vehicle_command.h index dca928efd2..3e220965d9 100644 --- a/apps/uORB/topics/vehicle_command.h +++ b/apps/uORB/topics/vehicle_command.h @@ -50,6 +50,10 @@ * @{ */ +enum PX4_CMD { + PX4_CMD_CONTROLLER_SELECTION = 1000, +}; + struct vehicle_command_s { float param1; /**< Parameter 1, as defined by MAVLink MAV_CMD enum. */ diff --git a/apps/uORB/topics/ardrone_motors_setpoint.h b/apps/uORB/topics/vehicle_rates_setpoint.h similarity index 70% rename from apps/uORB/topics/ardrone_motors_setpoint.h rename to apps/uORB/topics/vehicle_rates_setpoint.h index d0cb0ad733..3400e53999 100644 --- a/apps/uORB/topics/ardrone_motors_setpoint.h +++ b/apps/uORB/topics/vehicle_rates_setpoint.h @@ -33,12 +33,12 @@ ****************************************************************************/ /** - * @file ardrone_motors_setpoint.h - * Definition of the ardrone_motors_setpoint uORB topic. + * @file vehicle_rates_setpoint.h + * Definition of the vehicle rates setpoint topic */ -#ifndef TOPIC_ARDRONE_MOTORS_SETPOINT_H_ -#define TOPIC_ARDRONE_MOTORS_SETPOINT_H_ +#ifndef TOPIC_VEHICLE_RATES_SETPOINT_H_ +#define TOPIC_VEHICLE_RATES_SETPOINT_H_ #include #include "../uORB.h" @@ -48,23 +48,22 @@ * @{ */ -struct ardrone_motors_setpoint_s +struct vehicle_rates_setpoint_s { - uint64_t timestamp; //in microseconds since system start, is set whenever the writing thread stores new data + uint64_t timestamp; /**< in microseconds since system start */ - uint8_t group; /**< quadrotor group */ - uint8_t mode; /**< requested flight mode XXX define */ - float p1; /**< parameter 1: rate control: roll rate, att control: roll angle (in radians, NED) */ - float p2; /**< parameter 2: rate control: pitch rate, att control: pitch angle (in radians, NED) */ - float p3; /**< parameter 3: rate control: yaw rate, att control: yaw angle (in radians, NED) */ - float p4; /**< parameter 4: thrust, [0..1] */ -}; /**< AR.Drone low level motors */ + float roll; /**< body angular rates in NED frame */ + float pitch; /**< body angular rates in NED frame */ + float yaw; /**< body angular rates in NED frame */ + float thrust; /**< thrust normalized to 0..1 */ + +}; /**< vehicle_rates_setpoint */ /** * @} */ /* register this as object request broker structure */ -ORB_DECLARE(ardrone_motors_setpoint); +ORB_DECLARE(vehicle_rates_setpoint); #endif diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index cfde2ab53c..850029f015 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -127,7 +127,7 @@ struct vehicle_status_s bool flag_control_rates_enabled; /**< true if rates are stabilized */ bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ - bool flag_control_speed_enabled; /**< true if speed (implies direction) is controlled */ + bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */ bool flag_control_position_enabled; /**< true if position is controlled */ bool flag_preflight_gyro_calibration; /**< true if gyro calibration is requested */ From efcf146b6d22600341b55283b39f8b0a846dee09 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 19 Sep 2012 22:36:41 +0200 Subject: [PATCH 02/56] Updated EKF filter, untested --- apps/attitude_estimator_ekf/Makefile | 5 +- .../attitude_estimator_ekf_main.c | 62 +- .../codegen/attitudeKalmanfilter.c | 1031 +++++++++-------- .../codegen/attitudeKalmanfilter.h | 6 +- .../codegen/attitudeKalmanfilter_initialize.c | 2 +- .../codegen/attitudeKalmanfilter_initialize.h | 4 +- .../codegen/attitudeKalmanfilter_ref.rsp | 0 .../codegen/attitudeKalmanfilter_rtw.bat | 11 - .../codegen/attitudeKalmanfilter_rtw.mk | 328 ------ .../codegen/attitudeKalmanfilter_terminate.c | 2 +- .../codegen/attitudeKalmanfilter_terminate.h | 4 +- .../codegen/attitudeKalmanfilter_types.h | 2 +- .../codegen/buildInfo.mat | Bin 4114 -> 0 bytes apps/attitude_estimator_ekf/codegen/cross.c | 37 + apps/attitude_estimator_ekf/codegen/cross.h | 34 + apps/attitude_estimator_ekf/codegen/diag.c | 42 + apps/attitude_estimator_ekf/codegen/diag.h | 34 + apps/attitude_estimator_ekf/codegen/eye.c | 10 +- apps/attitude_estimator_ekf/codegen/eye.h | 6 +- apps/attitude_estimator_ekf/codegen/find.c | 77 ++ apps/attitude_estimator_ekf/codegen/find.h | 34 + .../attitude_estimator_ekf/codegen/mrdivide.c | 751 ++++++++++-- .../attitude_estimator_ekf/codegen/mrdivide.h | 6 +- apps/attitude_estimator_ekf/codegen/norm.c | 6 +- apps/attitude_estimator_ekf/codegen/norm.h | 4 +- .../attitude_estimator_ekf/codegen/rtGetInf.c | 2 +- .../attitude_estimator_ekf/codegen/rtGetInf.h | 2 +- .../attitude_estimator_ekf/codegen/rtGetNaN.c | 2 +- .../attitude_estimator_ekf/codegen/rtGetNaN.h | 2 +- .../codegen/rt_defines.h | 24 + .../codegen/rt_nonfinite.c | 2 +- .../codegen/rt_nonfinite.h | 2 +- .../codegen/rtw_proj.tmw | 1 - .../attitude_estimator_ekf/codegen/rtwtypes.h | 2 +- 34 files changed, 1595 insertions(+), 942 deletions(-) delete mode 100755 apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_ref.rsp delete mode 100755 apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.bat delete mode 100755 apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.mk delete mode 100755 apps/attitude_estimator_ekf/codegen/buildInfo.mat create mode 100755 apps/attitude_estimator_ekf/codegen/cross.c create mode 100755 apps/attitude_estimator_ekf/codegen/cross.h create mode 100755 apps/attitude_estimator_ekf/codegen/diag.c create mode 100755 apps/attitude_estimator_ekf/codegen/diag.h create mode 100755 apps/attitude_estimator_ekf/codegen/find.c create mode 100755 apps/attitude_estimator_ekf/codegen/find.h create mode 100755 apps/attitude_estimator_ekf/codegen/rt_defines.h delete mode 100755 apps/attitude_estimator_ekf/codegen/rtw_proj.tmw diff --git a/apps/attitude_estimator_ekf/Makefile b/apps/attitude_estimator_ekf/Makefile index cad20d3753..b4d62ed145 100644 --- a/apps/attitude_estimator_ekf/Makefile +++ b/apps/attitude_estimator_ekf/Makefile @@ -44,7 +44,10 @@ CSRCS = attitude_estimator_ekf_main.c \ codegen/rt_nonfinite.c \ codegen/rtGetInf.c \ codegen/rtGetNaN.c \ - codegen/norm.c + codegen/norm.c \ + codegen/find.c \ + codegen/diag.c \ + codegen/cross.c # XXX this is *horribly* broken INCLUDES += $(TOPDIR)/../mavlink/include/mavlink diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index 09cbdd4e94..6c18b044e5 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -80,21 +80,35 @@ static float dt = 1; /* 0, 0, -9.81, 1, 1, 1, wo (gyro offset), w */ /* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */ static float z_k[9] = {0}; /**< Measurement vector */ -static float x_aposteriori[12] = {0}; /**< */ -static float P_aposteriori[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f +static float x_aposteriori_k[9] = {0}; /**< */ +static float x_aposteriori[9] = {0}; +static float P_aposteriori_k[81] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 100.f, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 100.f, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 100.f, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 100.f, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 100.f, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 100.f, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 100.f, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 100.f, + 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, + }; +static float P_aposteriori[81] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 100.f, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 100.f, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 100.f, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 100.f, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 100.f, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 100.f, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 100.f, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 100.f, + 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, }; /**< init: diagonal matrix with big values */ -static float knownConst[7] = {1, 1, 1, 1, 1, 1, 1}; /**< knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */ +static float knownConst[20] = {1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}; /**< knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */ static float Rot_matrix[9] = {1.f, 0, 0, 0, 1.f, 0, 0, 0, 1.f @@ -203,13 +217,31 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) /* * function [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) */ + + //extern void attitudeKalmanfilter(real32_T dt, const int8_T updVect[9], const real32_T z_k_data[9], + // const int32_T z_k_sizes[1], const real32_T u[4], + // const real32_T x_aposteriori_k[9], const real32_T P_aposteriori_k[81], + // const real32_T knownConst[20], real32_T eulerAngles[3], + // real32_T Rot_matrix[9], real32_T x_aposteriori[9], + // real32_T P_aposteriori[81]); + + int8_t update_vect[9] = {1, 1, 1, 1, 1, 1, 1, 1, 1}; + float euler[3]; + int32_t z_k_sizes = 9; + float u[4] = {0.0f, 0.0f, 0.0f, 0.0f}; + uint64_t timing_start = hrt_absolute_time(); - attitudeKalmanfilter(dt, z_k, x_aposteriori, P_aposteriori, knownConst, Rot_matrix, x_aposteriori, P_aposteriori); + attitudeKalmanfilter(dt, update_vect, z_k, &z_k_sizes, u, x_aposteriori_k, P_aposteriori_k, knownConst, euler, + Rot_matrix, x_aposteriori, P_aposteriori); + /* swap values for next iteration */ + memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k)); + memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k)); uint64_t timing_diff = hrt_absolute_time() - timing_start; /* print rotation matrix every 200th time */ if (printcounter % 200 == 0) { printf("EKF attitude iteration: %d, runtime: %d us, dt: %d us (%d Hz)\n", loopcounter, (int)timing_diff, (int)(dt * 1000000.0f), (int)(1.0f / dt)); + printf("roll: %8.4f\tpitch: %8.4f\tyaw:%8.4f", euler[0], euler[1], euler[2]); printf("\n%d\t%d\t%d\n%d\t%d\t%d\n%d\t%d\t%d\n", (int)(Rot_matrix[0] * 100), (int)(Rot_matrix[1] * 100), (int)(Rot_matrix[2] * 100), (int)(Rot_matrix[3] * 100), (int)(Rot_matrix[4] * 100), (int)(Rot_matrix[5] * 100), (int)(Rot_matrix[6] * 100), (int)(Rot_matrix[7] * 100), (int)(Rot_matrix[8] * 100)); diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c index 4e275e3ee9..321e6874d0 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:22 2012 * */ @@ -11,8 +11,11 @@ #include "rt_nonfinite.h" #include "attitudeKalmanfilter.h" #include "norm.h" +#include "cross.h" #include "eye.h" #include "mrdivide.h" +#include "diag.h" +#include "find.h" /* Type Definitions */ @@ -23,613 +26,691 @@ /* Variable Definitions */ /* Function Declarations */ +static real32_T rt_atan2f_snf(real32_T u0, real32_T u1); /* Function Definitions */ +static real32_T rt_atan2f_snf(real32_T u0, real32_T u1) +{ + real32_T y; + if (rtIsNaNF(u0) || rtIsNaNF(u1)) { + y = ((real32_T)rtNaN); + } else if (rtIsInfF(u0) && rtIsInfF(u1)) { + y = (real32_T)atan2(u0 > 0.0F ? 1.0F : -1.0F, u1 > 0.0F ? 1.0F : -1.0F); + } else if (u1 == 0.0F) { + if (u0 > 0.0F) { + y = RT_PIF / 2.0F; + } else if (u0 < 0.0F) { + y = -(RT_PIF / 2.0F); + } else { + y = 0.0F; + } + } else { + y = (real32_T)atan2(u0, u1); + } + + return y; +} /* - * function [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) + * function [eulerAngles,Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,updVect,z_k,u,x_aposteriori_k,P_aposteriori_k,knownConst) */ -void attitudeKalmanfilter(real32_T dt, const real32_T z_k[9], const real32_T - x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T - knownConst[7], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T - P_aposteriori[144]) +void attitudeKalmanfilter(real32_T dt, const int8_T updVect[9], const real32_T + z_k_data[9], const int32_T z_k_sizes[1], const real32_T u[4], const real32_T + x_aposteriori_k[9], const real32_T P_aposteriori_k[81], const real32_T + knownConst[20], real32_T eulerAngles[3], real32_T Rot_matrix[9], real32_T + x_aposteriori[9], real32_T P_aposteriori[81]) { + int32_T udpIndVect_sizes; + real_T udpIndVect_data[9]; real32_T R_temp[9]; real_T dv0[9]; - real_T dv1[9]; - int32_T i; + int32_T ib; int32_T i0; - real32_T A_pred[144]; - real32_T x_apriori[12]; - real32_T b_A_pred[144]; + real32_T fv0[81]; + real32_T b_knownConst[27]; + real32_T fv1[9]; + real32_T c_knownConst[9]; + real_T dv1[9]; + real_T dv2[9]; + real32_T A_lin[81]; + real32_T x_n_b[3]; + real32_T K_k_data[81]; int32_T i1; - real32_T c_A_pred[144]; + real32_T b_A_lin[81]; static const int8_T iv0[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; - real32_T P_apriori[144]; - real32_T b_P_apriori[108]; - static const int8_T iv1[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, - 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1 }; - - real32_T K_k[108]; - static const int8_T iv2[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, + real32_T P_apriori[81]; + int32_T ia; + static const int8_T H_k_full[81] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; - real32_T fv0[81]; - real32_T fv1[81]; - real32_T fv2[81]; - real32_T B; - real_T dv2[144]; - real32_T b_B; - real32_T earth_z[3]; - real32_T y[3]; - real32_T earth_x[3]; + int8_T H_k_data[81]; + int32_T accUpt; + int32_T magUpt; + real32_T y; + real32_T y_k_data[9]; + int32_T P_apriori_sizes[2]; + int32_T H_k_sizes[2]; + int32_T K_k_sizes[2]; + real32_T b_y[9]; + real_T dv3[81]; + real32_T c_y; + real32_T z_n_b[3]; + real32_T y_n_b[3]; /* Extended Attitude Kalmanfilter */ - /* */ + /* */ /* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */ /* measurement vector z has the following entries [ax,ay,az||mx,my,mz||wmx,wmy,wmz]' */ /* knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */ /* */ /* [x_aposteriori,P_aposteriori] = AttKalman(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) */ /* */ - /* Example.... */ + /* Example.... */ /* */ /* $Author: Tobias Naegeli $ $Date: 2012 $ $Revision: 1 $ */ /* %define the matrices */ - /* 'attitudeKalmanfilter:19' acc_ProcessNoise=knownConst(1); */ - /* 'attitudeKalmanfilter:20' mag_ProcessNoise=knownConst(2); */ - /* 'attitudeKalmanfilter:21' ratesOffset_ProcessNoise=knownConst(3); */ - /* 'attitudeKalmanfilter:22' rates_ProcessNoise=knownConst(4); */ - /* 'attitudeKalmanfilter:25' acc_MeasurementNoise=knownConst(5); */ - /* 'attitudeKalmanfilter:26' mag_MeasurementNoise=knownConst(6); */ - /* 'attitudeKalmanfilter:27' rates_MeasurementNoise=knownConst(7); */ + /* tpred=0.005; */ + /* */ + /* A=[ 0, 0, 0, 0, 0, 0, 0, 0, 0; */ + /* 0, 0, 0, 0, 0, 0, 0, 0, 0; */ + /* 0, 0, 0, 0, 0, 0, 0, 0, 0; */ + /* 0, -1, 1, 0, 0, 0, 0, 0, 0; */ + /* 1, 0, -1, 0, 0, 0, 0, 0, 0; */ + /* -1, 1, 0, 0, 0, 0, 0, 0, 0; */ + /* 0, -1, 1, 0, 0, 0, 0, 0, 0; */ + /* 1, 0, -1, 0, 0, 0, 0, 0, 0; */ + /* -1, 1, 0, 0, 0, 0, 0, 0, 0]; */ + /* */ + /* */ + /* b=[70, 0, 0; */ + /* 0, 70, 0; */ + /* 0, 0, 0; */ + /* 0, 0, 0; */ + /* 0, 0, 0; */ + /* 0, 0, 0; */ + /* 0, 0, 0; */ + /* 0, 0, 0; */ + /* 0, 0, 0]; */ + /* */ + /* */ + /* C=[1, 0, 0, 0, 0, 0, 0, 0, 0; */ + /* 0, 1, 0, 0, 0, 0, 0, 0, 0; */ + /* 0, 0, 1, 0, 0, 0, 0, 0, 0; */ + /* 0, 0, 0, 1, 0, 0, 0, 0, 0; */ + /* 0, 0, 0, 0, 1, 0, 0, 0, 0; */ + /* 0, 0, 0, 0, 0, 1, 0, 0, 0; */ + /* 0, 0, 0, 0, 0, 0, 1, 0, 0; */ + /* 0, 0, 0, 0, 0, 0, 0, 1, 0; */ + /* 0, 0, 0, 0, 0, 0, 0, 0, 1]; */ + /* D=[]; */ + /* */ + /* */ + /* sys=ss(A,b,C,D); */ + /* */ + /* sysd=c2d(sys,tpred); */ + /* */ + /* */ + /* F=sysd.a; */ + /* */ + /* B=sysd.b; */ + /* */ + /* H=sysd.c; */ + /* 'attitudeKalmanfilter:68' udpIndVect=find(updVect); */ + find(updVect, udpIndVect_data, &udpIndVect_sizes); + + /* 'attitudeKalmanfilter:71' rates_ProcessNoiseMatrix=diag([knownConst(1),knownConst(1),knownConst(2)]); */ + /* 'attitudeKalmanfilter:72' acc_ProcessNoise=knownConst(3); */ + /* 'attitudeKalmanfilter:73' mag_ProcessNoise=knownConst(4); */ + /* 'attitudeKalmanfilter:74' rates_MeasurementNoise=knownConst(6); */ + /* 'attitudeKalmanfilter:75' acc_MeasurementNoise=knownConst(7); */ + /* 'attitudeKalmanfilter:76' mag_MeasurementNoise=knownConst(8); */ /* process noise covariance matrix */ - /* 'attitudeKalmanfilter:30' Q = [ eye(3)*acc_ProcessNoise, zeros(3), zeros(3), zeros(3); */ - /* 'attitudeKalmanfilter:31' zeros(3), eye(3)*mag_ProcessNoise, zeros(3), zeros(3); */ - /* 'attitudeKalmanfilter:32' zeros(3), zeros(3), eye(3)*ratesOffset_ProcessNoise, zeros(3); */ - /* 'attitudeKalmanfilter:33' zeros(3), zeros(3), zeros(3), eye(3)*rates_ProcessNoise]; */ - /* measurement noise covariance matrix */ - /* 'attitudeKalmanfilter:36' R = [ eye(3)*acc_MeasurementNoise, zeros(3), zeros(3); */ - /* 'attitudeKalmanfilter:37' zeros(3), eye(3)*mag_MeasurementNoise, zeros(3); */ - /* 'attitudeKalmanfilter:38' zeros(3), zeros(3), eye(3)*rates_MeasurementNoise]; */ + /* 'attitudeKalmanfilter:81' Q = [ rates_ProcessNoiseMatrix, zeros(3), zeros(3); */ + /* 'attitudeKalmanfilter:82' zeros(3), eye(3)*mag_ProcessNoise, zeros(3); */ + /* 'attitudeKalmanfilter:83' zeros(3), zeros(3), eye(3)*acc_ProcessNoise]; */ /* observation matrix */ - /* 'attitudeKalmanfilter:42' H_k=[ eye(3), zeros(3), zeros(3), zeros(3); */ - /* 'attitudeKalmanfilter:43' zeros(3), eye(3), zeros(3), zeros(3); */ - /* 'attitudeKalmanfilter:44' zeros(3), zeros(3), eye(3), eye(3)]; */ + /* 'attitudeKalmanfilter:89' H_k_full=[ eye(3), zeros(3), zeros(3); */ + /* 'attitudeKalmanfilter:90' zeros(3), eye(3), zeros(3); */ + /* 'attitudeKalmanfilter:91' zeros(3), zeros(3), eye(3)]; */ /* compute A(t,w) */ /* x_aposteriori_k[10,11,12] should be [p,q,r] */ /* R_temp=[1,-r, q */ /* r, 1, -p */ /* -q, p, 1] */ - /* 'attitudeKalmanfilter:53' R_temp=[1,-dt*x_aposteriori_k(12),dt*x_aposteriori_k(11); */ - /* 'attitudeKalmanfilter:54' dt*x_aposteriori_k(12),1,-dt*x_aposteriori_k(10); */ - /* 'attitudeKalmanfilter:55' -dt*x_aposteriori_k(11), dt*x_aposteriori_k(10),1]; */ + /* 'attitudeKalmanfilter:100' R_temp=[1, -dt*x_aposteriori_k(3), dt*x_aposteriori_k(2); */ + /* 'attitudeKalmanfilter:101' dt*x_aposteriori_k(3), 1, -dt*x_aposteriori_k(1); */ + /* 'attitudeKalmanfilter:102' -dt*x_aposteriori_k(2), dt*x_aposteriori_k(1), 1]; */ R_temp[0] = 1.0F; - R_temp[3] = -dt * x_aposteriori_k[11]; - R_temp[6] = dt * x_aposteriori_k[10]; - R_temp[1] = dt * x_aposteriori_k[11]; + R_temp[3] = -dt * x_aposteriori_k[2]; + R_temp[6] = dt * x_aposteriori_k[1]; + R_temp[1] = dt * x_aposteriori_k[2]; R_temp[4] = 1.0F; - R_temp[7] = -dt * x_aposteriori_k[9]; - R_temp[2] = -dt * x_aposteriori_k[10]; - R_temp[5] = dt * x_aposteriori_k[9]; + R_temp[7] = -dt * x_aposteriori_k[0]; + R_temp[2] = -dt * x_aposteriori_k[1]; + R_temp[5] = dt * x_aposteriori_k[0]; R_temp[8] = 1.0F; - /* strange, should not be transposed */ - /* 'attitudeKalmanfilter:58' A_pred=[R_temp', zeros(3), zeros(3), zeros(3); */ - /* 'attitudeKalmanfilter:59' zeros(3), R_temp', zeros(3), zeros(3); */ - /* 'attitudeKalmanfilter:60' zeros(3), zeros(3), eye(3), zeros(3); */ - /* 'attitudeKalmanfilter:61' zeros(3), zeros(3), zeros(3), eye(3)]; */ - eye(dv0); - eye(dv1); - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[i0 + 12 * i] = R_temp[i + 3 * i0]; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[i0 + 12 * (i + 3)] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[i0 + 12 * (i + 6)] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[i0 + 12 * (i + 9)] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * i) + 3] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 3)) + 3] = R_temp[i + 3 * i0]; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 6)) + 3] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 9)) + 3] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * i) + 6] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 3)) + 6] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 6)) + 6] = (real32_T)dv0[i0 + 3 * i]; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 9)) + 6] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * i) + 9] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 3)) + 9] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 6)) + 9] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 9)) + 9] = (real32_T)dv1[i0 + 3 * i]; - } - } - + /* 'attitudeKalmanfilter:106' A_pred=[eye(3), zeros(3), zeros(3); */ + /* 'attitudeKalmanfilter:107' zeros(3), R_temp', zeros(3); */ + /* 'attitudeKalmanfilter:108' zeros(3), zeros(3), R_temp']; */ + /* 'attitudeKalmanfilter:110' B_pred=[knownConst(9)*dt, 0, 0; */ + /* 'attitudeKalmanfilter:111' 0, knownConst(10)*dt, 0; */ + /* 'attitudeKalmanfilter:112' 0, 0, 0; */ + /* 'attitudeKalmanfilter:113' 0, 0, 0; */ + /* 'attitudeKalmanfilter:114' 0, 0, 0; */ + /* 'attitudeKalmanfilter:115' 0, 0, 0; */ + /* 'attitudeKalmanfilter:116' 0, 0, 0; */ + /* 'attitudeKalmanfilter:117' 0, 0, 0; */ + /* 'attitudeKalmanfilter:118' 0, 0, 0]; */ /* %prediction step */ - /* 'attitudeKalmanfilter:64' x_apriori=A_pred*x_aposteriori_k; */ - for (i = 0; i < 12; i++) { - x_apriori[i] = 0.0F; - for (i0 = 0; i0 < 12; i0++) { - x_apriori[i] += A_pred[i + 12 * i0] * x_aposteriori_k[i0]; + /* 'attitudeKalmanfilter:121' x_apriori=A_pred*x_aposteriori_k+B_pred*u(1:3); */ + eye(dv0); + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[i0 + 9 * ib] = (real32_T)dv0[i0 + 3 * ib]; } } + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[i0 + 9 * (ib + 3)] = 0.0F; + } + } + + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[i0 + 9 * (ib + 6)] = 0.0F; + } + } + + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[(i0 + 9 * ib) + 3] = 0.0F; + } + } + + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[(i0 + 9 * (ib + 3)) + 3] = R_temp[ib + 3 * i0]; + } + } + + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[(i0 + 9 * (ib + 6)) + 3] = 0.0F; + } + } + + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[(i0 + 9 * ib) + 6] = 0.0F; + } + } + + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[(i0 + 9 * (ib + 3)) + 6] = 0.0F; + } + } + + b_knownConst[0] = knownConst[8] * dt; + b_knownConst[9] = 0.0F; + b_knownConst[18] = 0.0F; + b_knownConst[1] = 0.0F; + b_knownConst[10] = knownConst[9] * dt; + b_knownConst[19] = 0.0F; + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[(i0 + 9 * (ib + 6)) + 6] = R_temp[ib + 3 * i0]; + } + + b_knownConst[2 + 9 * ib] = 0.0F; + b_knownConst[3 + 9 * ib] = 0.0F; + b_knownConst[4 + 9 * ib] = 0.0F; + b_knownConst[5 + 9 * ib] = 0.0F; + b_knownConst[6 + 9 * ib] = 0.0F; + b_knownConst[7 + 9 * ib] = 0.0F; + b_knownConst[8 + 9 * ib] = 0.0F; + } + + for (ib = 0; ib < 9; ib++) { + fv1[ib] = 0.0F; + for (i0 = 0; i0 < 9; i0++) { + fv1[ib] += fv0[ib + 9 * i0] * x_aposteriori_k[i0]; + } + + c_knownConst[ib] = 0.0F; + for (i0 = 0; i0 < 3; i0++) { + c_knownConst[ib] += b_knownConst[ib + 9 * i0] * u[i0]; + } + + x_aposteriori[ib] = fv1[ib] + c_knownConst[ib]; + } + /* linearization */ - /* 'attitudeKalmanfilter:67' acc_temp_mat=[0, dt*x_aposteriori_k(3), -dt*x_aposteriori_k(2); */ - /* 'attitudeKalmanfilter:68' -dt*x_aposteriori_k(3), 0, dt*x_aposteriori_k(1); */ - /* 'attitudeKalmanfilter:69' dt*x_aposteriori_k(2), -dt*x_aposteriori_k(1), 0]; */ - /* 'attitudeKalmanfilter:71' mag_temp_mat=[0, dt*x_aposteriori_k(6), -dt*x_aposteriori_k(5); */ - /* 'attitudeKalmanfilter:72' -dt*x_aposteriori_k(6), 0, dt*x_aposteriori_k(4); */ - /* 'attitudeKalmanfilter:73' dt*x_aposteriori_k(5), -dt*x_aposteriori_k(4), 0]; */ - /* 'attitudeKalmanfilter:75' A_lin=[R_temp', zeros(3), zeros(3), acc_temp_mat'; */ - /* 'attitudeKalmanfilter:76' zeros(3), R_temp', zeros(3), mag_temp_mat'; */ - /* 'attitudeKalmanfilter:77' zeros(3), zeros(3), eye(3), zeros(3); */ - /* 'attitudeKalmanfilter:78' zeros(3), zeros(3), zeros(3), eye(3)]; */ + /* 'attitudeKalmanfilter:125' temp_mat=[ 0, -dt, dt; */ + /* 'attitudeKalmanfilter:126' dt, 0, -dt; */ + /* 'attitudeKalmanfilter:127' -dt, dt, 0]; */ + R_temp[0] = 0.0F; + R_temp[3] = -dt; + R_temp[6] = dt; + R_temp[1] = dt; + R_temp[4] = 0.0F; + R_temp[7] = -dt; + R_temp[2] = -dt; + R_temp[5] = dt; + R_temp[8] = 0.0F; + + /* 'attitudeKalmanfilter:129' A_lin=[ eye(3), zeros(3), zeros(3); */ + /* 'attitudeKalmanfilter:130' temp_mat, eye(3) , zeros(3); */ + /* 'attitudeKalmanfilter:131' temp_mat, zeros(3), eye(3)]; */ eye(dv0); eye(dv1); - for (i = 0; i < 3; i++) { + eye(dv2); + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - A_pred[i0 + 12 * i] = R_temp[i + 3 * i0]; + A_lin[i0 + 9 * ib] = (real32_T)dv0[i0 + 3 * ib]; } } - for (i = 0; i < 3; i++) { + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - A_pred[i0 + 12 * (i + 3)] = 0.0F; + A_lin[i0 + 9 * (ib + 3)] = 0.0F; } } - for (i = 0; i < 3; i++) { + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - A_pred[i0 + 12 * (i + 6)] = 0.0F; + A_lin[i0 + 9 * (ib + 6)] = 0.0F; } } - A_pred[108] = 0.0F; - A_pred[109] = dt * x_aposteriori_k[2]; - A_pred[110] = -dt * x_aposteriori_k[1]; - A_pred[120] = -dt * x_aposteriori_k[2]; - A_pred[121] = 0.0F; - A_pred[122] = dt * x_aposteriori_k[0]; - A_pred[132] = dt * x_aposteriori_k[1]; - A_pred[133] = -dt * x_aposteriori_k[0]; - A_pred[134] = 0.0F; - for (i = 0; i < 3; i++) { + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * i) + 3] = 0.0F; + A_lin[(i0 + 9 * ib) + 3] = R_temp[i0 + 3 * ib]; } } - for (i = 0; i < 3; i++) { + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 3)) + 3] = R_temp[i + 3 * i0]; + A_lin[(i0 + 9 * (ib + 3)) + 3] = (real32_T)dv1[i0 + 3 * ib]; } } - for (i = 0; i < 3; i++) { + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 6)) + 3] = 0.0F; + A_lin[(i0 + 9 * (ib + 6)) + 3] = 0.0F; } } - A_pred[111] = 0.0F; - A_pred[112] = dt * x_aposteriori_k[5]; - A_pred[113] = -dt * x_aposteriori_k[4]; - A_pred[123] = -dt * x_aposteriori_k[5]; - A_pred[124] = 0.0F; - A_pred[125] = dt * x_aposteriori_k[3]; - A_pred[135] = dt * x_aposteriori_k[4]; - A_pred[136] = -dt * x_aposteriori_k[3]; - A_pred[137] = 0.0F; - for (i = 0; i < 3; i++) { + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * i) + 6] = 0.0F; + A_lin[(i0 + 9 * ib) + 6] = R_temp[i0 + 3 * ib]; } } - for (i = 0; i < 3; i++) { + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 3)) + 6] = 0.0F; + A_lin[(i0 + 9 * (ib + 3)) + 6] = 0.0F; } } - for (i = 0; i < 3; i++) { + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 6)) + 6] = (real32_T)dv0[i0 + 3 * i]; + A_lin[(i0 + 9 * (ib + 6)) + 6] = (real32_T)dv2[i0 + 3 * ib]; } } - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 9)) + 6] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * i) + 9] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 3)) + 9] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 6)) + 9] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 9)) + 9] = (real32_T)dv1[i0 + 3 * i]; - } - } - - /* 'attitudeKalmanfilter:81' P_apriori=A_lin*P_aposteriori_k*A_lin'+Q; */ - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - b_A_pred[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - b_A_pred[i + 12 * i0] += A_pred[i + 12 * i1] * P_aposteriori_k[i1 + 12 * + /* 'attitudeKalmanfilter:134' P_apriori=A_lin*P_aposteriori_k*A_lin'+Q; */ + x_n_b[0] = knownConst[0]; + x_n_b[1] = knownConst[0]; + x_n_b[2] = knownConst[1]; + diag(x_n_b, R_temp); + for (ib = 0; ib < 9; ib++) { + for (i0 = 0; i0 < 9; i0++) { + K_k_data[ib + 9 * i0] = 0.0F; + for (i1 = 0; i1 < 9; i1++) { + K_k_data[ib + 9 * i0] += A_lin[ib + 9 * i1] * P_aposteriori_k[i1 + 9 * i0]; } } - for (i0 = 0; i0 < 12; i0++) { - c_A_pred[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - c_A_pred[i + 12 * i0] += b_A_pred[i + 12 * i1] * A_pred[i0 + 12 * i1]; + for (i0 = 0; i0 < 9; i0++) { + b_A_lin[ib + 9 * i0] = 0.0F; + for (i1 = 0; i1 < 9; i1++) { + b_A_lin[ib + 9 * i0] += K_k_data[ib + 9 * i1] * A_lin[i0 + 9 * i1]; } } } - for (i = 0; i < 3; i++) { + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - b_A_pred[i0 + 12 * i] = (real32_T)iv0[i0 + 3 * i] * knownConst[0]; + K_k_data[i0 + 9 * ib] = R_temp[i0 + 3 * ib]; } } - for (i = 0; i < 3; i++) { + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - b_A_pred[i0 + 12 * (i + 3)] = 0.0F; + K_k_data[i0 + 9 * (ib + 3)] = 0.0F; } } - for (i = 0; i < 3; i++) { + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - b_A_pred[i0 + 12 * (i + 6)] = 0.0F; + K_k_data[i0 + 9 * (ib + 6)] = 0.0F; } } - for (i = 0; i < 3; i++) { + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - b_A_pred[i0 + 12 * (i + 9)] = 0.0F; + K_k_data[(i0 + 9 * ib) + 3] = 0.0F; } } - for (i = 0; i < 3; i++) { + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - b_A_pred[(i0 + 12 * i) + 3] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - b_A_pred[(i0 + 12 * (i + 3)) + 3] = (real32_T)iv0[i0 + 3 * i] * - knownConst[1]; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - b_A_pred[(i0 + 12 * (i + 6)) + 3] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - b_A_pred[(i0 + 12 * (i + 9)) + 3] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - b_A_pred[(i0 + 12 * i) + 6] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - b_A_pred[(i0 + 12 * (i + 3)) + 6] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - b_A_pred[(i0 + 12 * (i + 6)) + 6] = (real32_T)iv0[i0 + 3 * i] * - knownConst[2]; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - b_A_pred[(i0 + 12 * (i + 9)) + 6] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - b_A_pred[(i0 + 12 * i) + 9] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - b_A_pred[(i0 + 12 * (i + 3)) + 9] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - b_A_pred[(i0 + 12 * (i + 6)) + 9] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - b_A_pred[(i0 + 12 * (i + 9)) + 9] = (real32_T)iv0[i0 + 3 * i] * + K_k_data[(i0 + 9 * (ib + 3)) + 3] = (real32_T)iv0[i0 + 3 * ib] * knownConst[3]; } } - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - P_apriori[i0 + 12 * i] = c_A_pred[i0 + 12 * i] + b_A_pred[i0 + 12 * i]; + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + K_k_data[(i0 + 9 * (ib + 6)) + 3] = 0.0F; } } - /* %update step */ - /* 'attitudeKalmanfilter:86' y_k=z_k-H_k*x_apriori; */ - /* 'attitudeKalmanfilter:87' S_k=H_k*P_apriori*H_k'+R; */ - /* 'attitudeKalmanfilter:88' K_k=(P_apriori*H_k'/(S_k)); */ - for (i = 0; i < 12; i++) { + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + K_k_data[(i0 + 9 * ib) + 6] = 0.0F; + } + } + + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + K_k_data[(i0 + 9 * (ib + 3)) + 6] = 0.0F; + } + } + + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + K_k_data[(i0 + 9 * (ib + 6)) + 6] = (real32_T)iv0[i0 + 3 * ib] * + knownConst[2]; + } + } + + for (ib = 0; ib < 9; ib++) { for (i0 = 0; i0 < 9; i0++) { - b_P_apriori[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - b_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)iv1[i1 + - 12 * i0]; - } + P_apriori[i0 + 9 * ib] = b_A_lin[i0 + 9 * ib] + K_k_data[i0 + 9 * ib]; } } - for (i = 0; i < 9; i++) { - for (i0 = 0; i0 < 12; i0++) { - K_k[i + 9 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - K_k[i + 9 * i0] += (real32_T)iv2[i + 9 * i1] * P_apriori[i1 + 12 * i0]; + /* 'attitudeKalmanfilter:137' if ~isempty(udpIndVect)==1 */ + if (!(udpIndVect_sizes == 0) == 1) { + /* 'attitudeKalmanfilter:138' H_k= H_k_full(udpIndVect,:); */ + for (ib = 0; ib < 9; ib++) { + ia = udpIndVect_sizes - 1; + for (i0 = 0; i0 <= ia; i0++) { + H_k_data[i0 + udpIndVect_sizes * ib] = H_k_full[((int32_T) + udpIndVect_data[i0] + 9 * ib) - 1]; } } - for (i0 = 0; i0 < 9; i0++) { - fv0[i + 9 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - fv0[i + 9 * i0] += K_k[i + 9 * i1] * (real32_T)iv1[i1 + 12 * i0]; - } - } - } + /* %update step */ + /* 'attitudeKalmanfilter:140' accUpt=1; */ + accUpt = 1; - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - fv1[i0 + 9 * i] = (real32_T)iv0[i0 + 3 * i] * knownConst[4]; - } - } + /* 'attitudeKalmanfilter:141' magUpt=1; */ + magUpt = 1; - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - fv1[i0 + 9 * (i + 3)] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - fv1[i0 + 9 * (i + 6)] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - fv1[(i0 + 9 * i) + 3] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - fv1[(i0 + 9 * (i + 3)) + 3] = (real32_T)iv0[i0 + 3 * i] * knownConst[5]; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - fv1[(i0 + 9 * (i + 6)) + 3] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - fv1[(i0 + 9 * i) + 6] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - fv1[(i0 + 9 * (i + 3)) + 6] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - fv1[(i0 + 9 * (i + 6)) + 6] = (real32_T)iv0[i0 + 3 * i] * knownConst[6]; - } - } - - for (i = 0; i < 9; i++) { - for (i0 = 0; i0 < 9; i0++) { - fv2[i0 + 9 * i] = fv0[i0 + 9 * i] + fv1[i0 + 9 * i]; - } - } - - mrdivide(b_P_apriori, fv2, K_k); - - /* 'attitudeKalmanfilter:91' x_aposteriori=x_apriori+K_k*y_k; */ - for (i = 0; i < 9; i++) { - B = 0.0F; - for (i0 = 0; i0 < 12; i0++) { - B += (real32_T)iv2[i + 9 * i0] * x_apriori[i0]; - } - - R_temp[i] = z_k[i] - B; - } - - for (i = 0; i < 12; i++) { - B = 0.0F; - for (i0 = 0; i0 < 9; i0++) { - B += K_k[i + 12 * i0] * R_temp[i0]; - } - - x_aposteriori[i] = x_apriori[i] + B; - } - - /* 'attitudeKalmanfilter:92' P_aposteriori=(eye(12)-K_k*H_k)*P_apriori; */ - b_eye(dv2); - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - B = 0.0F; - for (i1 = 0; i1 < 9; i1++) { - B += K_k[i + 12 * i1] * (real32_T)iv2[i1 + 9 * i0]; + /* 'attitudeKalmanfilter:142' y_k=z_k-H_k*x_apriori; */ + ia = udpIndVect_sizes - 1; + for (ib = 0; ib <= ia; ib++) { + y = 0.0F; + for (i0 = 0; i0 < 9; i0++) { + y += (real32_T)H_k_data[ib + udpIndVect_sizes * i0] * x_aposteriori[i0]; } - A_pred[i + 12 * i0] = (real32_T)dv2[i + 12 * i0] - B; + y_k_data[ib] = z_k_data[ib] - y; } - } - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - P_aposteriori[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - P_aposteriori[i + 12 * i0] += A_pred[i + 12 * i1] * P_apriori[i1 + 12 * - i0]; + /* 'attitudeKalmanfilter:143' if updVect(4)==1 */ + if (updVect[3] == 1) { + /* 'attitudeKalmanfilter:144' if (abs(norm(z_k(4:6))-knownConst(12))>knownConst(14)) */ + for (ib = 0; ib < 3; ib++) { + x_n_b[ib] = z_k_data[ib + 3]; + } + + if ((real32_T)fabs(norm(x_n_b) - knownConst[11]) > knownConst[13]) { + /* 'attitudeKalmanfilter:145' accUpt=10000; */ + accUpt = 10000; } } + + /* 'attitudeKalmanfilter:149' if updVect(7)==1 */ + if (updVect[6] == 1) { + /* 'attitudeKalmanfilter:150' if (abs(norm(z_k(7:9))-knownConst(13))>knownConst(15)) */ + for (ib = 0; ib < 3; ib++) { + x_n_b[ib] = z_k_data[ib + 6]; + } + + if ((real32_T)fabs(norm(x_n_b) - knownConst[12]) > knownConst[14]) { + /* 'attitudeKalmanfilter:152' magUpt=10000; */ + magUpt = 10000; + } + } + + /* measurement noise covariance matrix */ + /* 'attitudeKalmanfilter:157' R = [ eye(3)*rates_MeasurementNoise, zeros(3), zeros(3); */ + /* 'attitudeKalmanfilter:158' zeros(3), eye(3)*acc_MeasurementNoise*accUpt, zeros(3); */ + /* 'attitudeKalmanfilter:159' zeros(3), zeros(3), eye(3)*mag_MeasurementNoise*magUpt]; */ + /* 'attitudeKalmanfilter:161' S_k=H_k*P_apriori*H_k'+R(udpIndVect,udpIndVect); */ + /* 'attitudeKalmanfilter:162' K_k=(P_apriori*H_k'/(S_k)); */ + P_apriori_sizes[0] = 9; + P_apriori_sizes[1] = udpIndVect_sizes; + for (ib = 0; ib < 9; ib++) { + ia = udpIndVect_sizes - 1; + for (i0 = 0; i0 <= ia; i0++) { + b_A_lin[ib + 9 * i0] = 0.0F; + for (i1 = 0; i1 < 9; i1++) { + b_A_lin[ib + 9 * i0] += P_apriori[ib + 9 * i1] * (real32_T)H_k_data[i0 + + udpIndVect_sizes * i1]; + } + } + } + + ia = udpIndVect_sizes - 1; + for (ib = 0; ib <= ia; ib++) { + for (i0 = 0; i0 < 9; i0++) { + K_k_data[ib + udpIndVect_sizes * i0] = 0.0F; + for (i1 = 0; i1 < 9; i1++) { + K_k_data[ib + udpIndVect_sizes * i0] += (real32_T)H_k_data[ib + + udpIndVect_sizes * i1] * P_apriori[i1 + 9 * i0]; + } + } + } + + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[i0 + 9 * ib] = (real32_T)iv0[i0 + 3 * ib] * knownConst[5]; + } + } + + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[i0 + 9 * (ib + 3)] = 0.0F; + } + } + + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[i0 + 9 * (ib + 6)] = 0.0F; + } + } + + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[(i0 + 9 * ib) + 3] = 0.0F; + } + } + + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[(i0 + 9 * (ib + 3)) + 3] = (real32_T)iv0[i0 + 3 * ib] * knownConst[6] + * (real32_T)accUpt; + } + } + + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[(i0 + 9 * (ib + 6)) + 3] = 0.0F; + } + } + + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[(i0 + 9 * ib) + 6] = 0.0F; + } + } + + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[(i0 + 9 * (ib + 3)) + 6] = 0.0F; + } + } + + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[(i0 + 9 * (ib + 6)) + 6] = (real32_T)iv0[i0 + 3 * ib] * knownConst[7] + * (real32_T)magUpt; + } + } + + H_k_sizes[0] = udpIndVect_sizes; + H_k_sizes[1] = udpIndVect_sizes; + ia = udpIndVect_sizes - 1; + for (ib = 0; ib <= ia; ib++) { + accUpt = udpIndVect_sizes - 1; + for (i0 = 0; i0 <= accUpt; i0++) { + y = 0.0F; + for (i1 = 0; i1 < 9; i1++) { + y += K_k_data[ib + udpIndVect_sizes * i1] * (real32_T)H_k_data[i0 + + udpIndVect_sizes * i1]; + } + + A_lin[ib + H_k_sizes[0] * i0] = y + fv0[((int32_T)udpIndVect_data[ib] + + 9 * ((int32_T)udpIndVect_data[i0] - 1)) - 1]; + } + } + + mrdivide(b_A_lin, P_apriori_sizes, A_lin, H_k_sizes, K_k_data, K_k_sizes); + + /* 'attitudeKalmanfilter:165' x_aposteriori=x_apriori+K_k*y_k; */ + if ((K_k_sizes[1] == 1) || (udpIndVect_sizes == 1)) { + for (ib = 0; ib < 9; ib++) { + b_y[ib] = 0.0F; + ia = udpIndVect_sizes - 1; + for (i0 = 0; i0 <= ia; i0++) { + b_y[ib] += K_k_data[ib + K_k_sizes[0] * i0] * y_k_data[i0]; + } + } + } else { + for (accUpt = 0; accUpt < 9; accUpt++) { + b_y[accUpt] = 0.0F; + } + + magUpt = -1; + for (ib = 0; ib + 1 <= K_k_sizes[1]; ib++) { + if ((real_T)y_k_data[ib] != 0.0) { + ia = magUpt; + for (accUpt = 0; accUpt < 9; accUpt++) { + ia++; + b_y[accUpt] += y_k_data[ib] * K_k_data[ia]; + } + } + + magUpt += 9; + } + } + + for (ib = 0; ib < 9; ib++) { + x_aposteriori[ib] += b_y[ib]; + } + + /* 'attitudeKalmanfilter:166' P_aposteriori=(eye(9)-K_k*H_k)*P_apriori; */ + b_eye(dv3); + for (ib = 0; ib < 9; ib++) { + for (i0 = 0; i0 < 9; i0++) { + y = 0.0F; + ia = K_k_sizes[1] - 1; + for (i1 = 0; i1 <= ia; i1++) { + y += K_k_data[ib + K_k_sizes[0] * i1] * (real32_T)H_k_data[i1 + + udpIndVect_sizes * i0]; + } + + fv0[ib + 9 * i0] = (real32_T)dv3[ib + 9 * i0] - y; + } + } + + for (ib = 0; ib < 9; ib++) { + for (i0 = 0; i0 < 9; i0++) { + P_aposteriori[ib + 9 * i0] = 0.0F; + for (i1 = 0; i1 < 9; i1++) { + P_aposteriori[ib + 9 * i0] += fv0[ib + 9 * i1] * P_apriori[i1 + 9 * i0]; + } + } + } + } else { + /* 'attitudeKalmanfilter:167' else */ + /* 'attitudeKalmanfilter:168' x_aposteriori=x_apriori; */ + /* 'attitudeKalmanfilter:169' P_aposteriori=P_apriori; */ + memcpy((void *)&P_aposteriori[0], (void *)&P_apriori[0], 81U * sizeof + (real32_T)); } - /* %Rotation matrix generation */ - /* 'attitudeKalmanfilter:97' earth_z=x_aposteriori(1:3)/norm(x_aposteriori(1:3)); */ - B = norm(*(real32_T (*)[3])&x_aposteriori[0]); + /* %% euler anglels extraction */ + /* 'attitudeKalmanfilter:175' z_n_b = -x_aposteriori(4:6)./norm(x_aposteriori(4:6)); */ + y = norm(*(real32_T (*)[3])&x_aposteriori[3]); - /* 'attitudeKalmanfilter:98' earth_x=cross(earth_z,x_aposteriori(4:6)/norm(x_aposteriori(4:6))); */ - b_B = norm(*(real32_T (*)[3])&x_aposteriori[3]); - for (i = 0; i < 3; i++) { - earth_z[i] = x_aposteriori[i] / B; - y[i] = x_aposteriori[i + 3] / b_B; + /* 'attitudeKalmanfilter:176' m_n_b = x_aposteriori(7:9)./norm(x_aposteriori(7:9)); */ + c_y = norm(*(real32_T (*)[3])&x_aposteriori[6]); + + /* 'attitudeKalmanfilter:178' y_n_b=cross(z_n_b,m_n_b); */ + for (accUpt = 0; accUpt < 3; accUpt++) { + z_n_b[accUpt] = -x_aposteriori[accUpt + 3] / y; + x_n_b[accUpt] = x_aposteriori[accUpt + 6] / c_y; } - earth_x[0] = earth_z[1] * y[2] - earth_z[2] * y[1]; - earth_x[1] = earth_z[2] * y[0] - earth_z[0] * y[2]; - earth_x[2] = earth_z[0] * y[1] - earth_z[1] * y[0]; + cross(z_n_b, x_n_b, y_n_b); - /* 'attitudeKalmanfilter:99' earth_y=cross(earth_x,earth_z); */ - /* 'attitudeKalmanfilter:101' Rot_matrix=[earth_x,earth_y,earth_z]; */ - y[0] = earth_x[1] * earth_z[2] - earth_x[2] * earth_z[1]; - y[1] = earth_x[2] * earth_z[0] - earth_x[0] * earth_z[2]; - y[2] = earth_x[0] * earth_z[1] - earth_x[1] * earth_z[0]; - for (i = 0; i < 3; i++) { - Rot_matrix[i] = earth_x[i]; - Rot_matrix[3 + i] = y[i]; - Rot_matrix[6 + i] = earth_z[i]; + /* 'attitudeKalmanfilter:179' y_n_b=y_n_b./norm(y_n_b); */ + y = norm(y_n_b); + for (ib = 0; ib < 3; ib++) { + y_n_b[ib] /= y; } + + /* 'attitudeKalmanfilter:181' x_n_b=(cross(y_n_b,z_n_b)); */ + cross(y_n_b, z_n_b, x_n_b); + + /* 'attitudeKalmanfilter:182' x_n_b=x_n_b./norm(x_n_b); */ + y = norm(x_n_b); + for (ib = 0; ib < 3; ib++) { + /* 'attitudeKalmanfilter:188' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */ + Rot_matrix[ib] = x_n_b[ib] / y; + Rot_matrix[3 + ib] = y_n_b[ib]; + Rot_matrix[6 + ib] = z_n_b[ib]; + } + + /* 'attitudeKalmanfilter:192' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */ + /* 'attitudeKalmanfilter:193' theta=-asin(Rot_matrix(1,3)); */ + /* 'attitudeKalmanfilter:194' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */ + /* 'attitudeKalmanfilter:195' eulerAngles=[phi;theta;psi]; */ + eulerAngles[0] = rt_atan2f_snf(Rot_matrix[7], Rot_matrix[8]); + eulerAngles[1] = -(real32_T)asin(Rot_matrix[6]); + eulerAngles[2] = rt_atan2f_snf(Rot_matrix[3], Rot_matrix[0]); } /* End of code generation (attitudeKalmanfilter.c) */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h index 7aa3d048b7..8207aa5c53 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:22 2012 * */ @@ -14,6 +14,8 @@ #include #include #include +#include "rt_defines.h" +#include "rt_nonfinite.h" #include "rtwtypes.h" #include "attitudeKalmanfilter_types.h" @@ -27,6 +29,6 @@ /* Variable Definitions */ /* Function Declarations */ -extern void attitudeKalmanfilter(real32_T dt, const real32_T z_k[9], const real32_T x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T knownConst[7], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T P_aposteriori[144]); +extern void attitudeKalmanfilter(real32_T dt, const int8_T updVect[9], const real32_T z_k_data[9], const int32_T z_k_sizes[1], const real32_T u[4], const real32_T x_aposteriori_k[9], const real32_T P_aposteriori_k[81], const real32_T knownConst[20], real32_T eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[9], real32_T P_aposteriori[81]); #endif /* End of code generation (attitudeKalmanfilter.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c index 2800191c32..abf1519a67 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter_initialize' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:23 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h index d2d97bb3b9..20186f183c 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter_initialize' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:24 2012 * */ @@ -14,6 +14,8 @@ #include #include #include +#include "rt_defines.h" +#include "rt_nonfinite.h" #include "rtwtypes.h" #include "attitudeKalmanfilter_types.h" diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_ref.rsp b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_ref.rsp deleted file mode 100755 index e69de29bb2..0000000000 diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.bat b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.bat deleted file mode 100755 index 76fb78ca75..0000000000 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.bat +++ /dev/null @@ -1,11 +0,0 @@ -@echo off -call "%VS100COMNTOOLS%..\..\VC\vcvarsall.bat" AMD64 - -cd . -nmake -f attitudeKalmanfilter_rtw.mk GENERATE_REPORT=1 ADD_MDL_NAME_TO_GLOBALS=0 -@if errorlevel 1 goto error_exit -exit /B 0 - -:error_exit -echo The make command returned an error of %errorlevel% -An_error_occurred_during_the_call_to_make diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.mk b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.mk deleted file mode 100755 index b2123704b1..0000000000 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.mk +++ /dev/null @@ -1,328 +0,0 @@ -# Copyright 1994-2010 The MathWorks, Inc. -# -# File : xrt_vcx64.tmf $Revision: 1.1.6.1 $ -# -# Abstract: -# Code generation template makefile for building a Windows-based, -# stand-alone real-time version of MATLAB-code using generated C/C++ -# code and the -# Microsoft Visual C/C++ compiler version 8, 9, 10 for x64 -# -# Note that this template is automatically customized by the -# code generation build procedure to create ".mk" -# -# The following defines can be used to modify the behavior of the -# build: -# -# OPT_OPTS - Optimization option. See DEFAULT_OPT_OPTS in -# vctools.mak for default. -# OPTS - User specific options. -# CPP_OPTS - C++ compiler options. -# USER_SRCS - Additional user sources, such as files needed by -# S-functions. -# USER_INCLUDES - Additional include paths -# (i.e. USER_INCLUDES="-Iwhere-ever -Iwhere-ever2") -# -# To enable debugging: -# set OPT_OPTS=-Zi (may vary with compiler version, see compiler doc) -# modify tmf LDFLAGS to include /DEBUG -# - -#------------------------ Macros read by make_rtw ----------------------------- -# -# The following macros are read by the code generation build procedure: -# -# MAKECMD - This is the command used to invoke the make utility -# HOST - What platform this template makefile is targeted for -# (i.e. PC or UNIX) -# BUILD - Invoke make from the code generation build procedure -# (yes/no)? -# SYS_TARGET_FILE - Name of system target file. - -MAKECMD = nmake -HOST = PC -BUILD = yes -SYS_TARGET_FILE = ert.tlc -BUILD_SUCCESS = ^#^#^# Created -COMPILER_TOOL_CHAIN = vcx64 - -#---------------------- Tokens expanded by make_rtw --------------------------- -# -# The following tokens, when wrapped with "|>" and "<|" are expanded by the -# code generation build procedure. -# -# MODEL_NAME - Name of the Simulink block diagram -# MODEL_MODULES - Any additional generated source modules -# MAKEFILE_NAME - Name of makefile created from template makefile .mk -# MATLAB_ROOT - Path to where MATLAB is installed. -# MATLAB_BIN - Path to MATLAB executable. -# S_FUNCTIONS - List of S-functions. -# S_FUNCTIONS_LIB - List of S-functions libraries to link. -# SOLVER - Solver source file name -# NUMST - Number of sample times -# TID01EQ - yes (1) or no (0): Are sampling rates of continuous task -# (tid=0) and 1st discrete task equal. -# NCSTATES - Number of continuous states -# BUILDARGS - Options passed in at the command line. -# MULTITASKING - yes (1) or no (0): Is solver mode multitasking -# EXT_MODE - yes (1) or no (0): Build for external mode -# TMW_EXTMODE_TESTING - yes (1) or no (0): Build ext_test.c for external mode -# testing. -# EXTMODE_TRANSPORT - Index of transport mechanism (e.g. tcpip, serial) for extmode -# EXTMODE_STATIC - yes (1) or no (0): Use static instead of dynamic mem alloc. -# EXTMODE_STATIC_SIZE - Size of static memory allocation buffer. - -MODEL = attitudeKalmanfilter -MODULES = attitudeKalmanfilter.c eye.c mrdivide.c norm.c attitudeKalmanfilter_initialize.c attitudeKalmanfilter_terminate.c rt_nonfinite.c rtGetNaN.c rtGetInf.c -MAKEFILE = attitudeKalmanfilter_rtw.mk -MATLAB_ROOT = C:\Program Files\MATLAB\R2011a -ALT_MATLAB_ROOT = C:\PROGRA~1\MATLAB\R2011a -MATLAB_BIN = C:\Program Files\MATLAB\R2011a\bin -ALT_MATLAB_BIN = C:\PROGRA~1\MATLAB\R2011a\bin -S_FUNCTIONS = -S_FUNCTIONS_LIB = -SOLVER = -NUMST = 1 -TID01EQ = 0 -NCSTATES = 0 -BUILDARGS = GENERATE_REPORT=1 ADD_MDL_NAME_TO_GLOBALS=0 -MULTITASKING = 0 -EXT_MODE = 0 -TMW_EXTMODE_TESTING = 0 -EXTMODE_TRANSPORT = 0 -EXTMODE_STATIC = 0 -EXTMODE_STATIC_SIZE = 1000000 - -MODELREFS = -SHARED_SRC = -SHARED_SRC_DIR = -SHARED_BIN_DIR = -SHARED_LIB = -TARGET_LANG_EXT = c -OPTIMIZATION_FLAGS = /O2 /Oy- -ADDITIONAL_LDFLAGS = - -#--------------------------- Model and reference models ----------------------- -MODELLIB = attitudeKalmanfilter.lib -MODELREF_LINK_LIBS = -MODELREF_LINK_RSPFILE = attitudeKalmanfilter_ref.rsp -MODELREF_INC_PATH = -RELATIVE_PATH_TO_ANCHOR = F:\CODEGE~1 -MODELREF_TARGET_TYPE = RTW - -!if "$(MATLAB_ROOT)" != "$(ALT_MATLAB_ROOT)" -MATLAB_ROOT = $(ALT_MATLAB_ROOT) -!endif -!if "$(MATLAB_BIN)" != "$(ALT_MATLAB_BIN)" -MATLAB_BIN = $(ALT_MATLAB_BIN) -!endif - -#--------------------------- Tool Specifications ------------------------------ - -CPU = AMD64 -!include $(MATLAB_ROOT)\rtw\c\tools\vctools.mak -APPVER = 5.02 - -PERL = $(MATLAB_ROOT)\sys\perl\win32\bin\perl -#------------------------------ Include/Lib Path ------------------------------ - -MATLAB_INCLUDES = $(MATLAB_ROOT)\simulink\include -MATLAB_INCLUDES = $(MATLAB_INCLUDES);$(MATLAB_ROOT)\extern\include -MATLAB_INCLUDES = $(MATLAB_INCLUDES);$(MATLAB_ROOT)\rtw\c\src -MATLAB_INCLUDES = $(MATLAB_INCLUDES);$(MATLAB_ROOT)\rtw\c\src\ext_mode\common - -# Additional file include paths - -MATLAB_INCLUDES = $(MATLAB_INCLUDES);F:\codegenerationMatlabAttFilter\codegen\lib\attitudeKalmanfilter -MATLAB_INCLUDES = $(MATLAB_INCLUDES);F:\codegenerationMatlabAttFilter - -INCLUDE = .;$(RELATIVE_PATH_TO_ANCHOR);$(MATLAB_INCLUDES);$(INCLUDE);$(MODELREF_INC_PATH) - -!if "$(SHARED_SRC_DIR)" != "" -INCLUDE = $(INCLUDE);$(SHARED_SRC_DIR) -!endif - -#------------------------ External mode --------------------------------------- -# Uncomment -DVERBOSE to have information printed to stdout -# To add a new transport layer, see the comments in -# /toolbox/simulink/simulink/extmode_transports.m -!if $(EXT_MODE) == 1 -EXT_CC_OPTS = -DEXT_MODE # -DVERBOSE -!if $(EXTMODE_TRANSPORT) == 0 #tcpip -EXT_SRC = updown.c rtiostream_interface.c rtiostream_tcpip.c -EXT_LIB = wsock32.lib -!endif -!if $(EXTMODE_TRANSPORT) == 1 #serial_win32 -EXT_SRC = ext_svr.c updown.c ext_work.c ext_svr_serial_transport.c -EXT_SRC = $(EXT_SRC) ext_serial_pkt.c ext_serial_win32_port.c -EXT_LIB = -!endif -!if $(TMW_EXTMODE_TESTING) == 1 -EXT_SRC = $(EXT_SRC) ext_test.c -EXT_CC_OPTS = $(EXT_CC_OPTS) -DTMW_EXTMODE_TESTING -!endif -!if $(EXTMODE_STATIC) == 1 -EXT_SRC = $(EXT_SRC) mem_mgr.c -EXT_CC_OPTS = $(EXT_CC_OPTS) -DEXTMODE_STATIC -DEXTMODE_STATIC_SIZE=$(EXTMODE_STATIC_SIZE) -!endif -!else -EXT_SRC = -EXT_CC_OPTS = -EXT_LIB = -!endif - -#------------------------ rtModel ---------------------------------------------- - -RTM_CC_OPTS = -DUSE_RTMODEL - -#----------------- Compiler and Linker Options -------------------------------- - -# Optimization Options -# Set OPT_OPTS=-Zi for debugging (may depend on compiler version) -OPT_OPTS = $(DEFAULT_OPT_OPTS) - -# General User Options -OPTS = - -!if "$(OPTIMIZATION_FLAGS)" != "" -CC_OPTS = $(OPTS) $(EXT_CC_OPTS) $(RTM_CC_OPTS) $(OPTIMIZATION_FLAGS) -!else -CC_OPTS = $(OPT_OPTS) $(OPTS) $(EXT_CC_OPTS) $(RTM_CC_OPTS) -!endif - -CPP_REQ_DEFINES = -DMODEL=$(MODEL) -DRT -DNUMST=$(NUMST) \ - -DTID01EQ=$(TID01EQ) -DNCSTATES=$(NCSTATES) \ - -DMT=$(MULTITASKING) -DHAVESTDIO - -# Uncomment this line to move warning level to W4 -# cflags = $(cflags:W3=W4) -CFLAGS = $(CC_OPTS) $(CPP_REQ_DEFINES) $(USER_INCLUDES) \ - $(cflags) $(cvarsmt) /wd4996 -CPPFLAGS = $(CPP_OPTS) $(CC_OPTS) $(CPP_REQ_DEFINES) $(USER_INCLUDES) \ - $(cflags) $(cvarsmt) /wd4996 /EHsc- -LDFLAGS = $(ldebug) $(conflags) $(EXT_LIB) $(conlibs) libcpmt.lib $(ADDITIONAL_LDFLAGS) - -# libcpmt.lib is the multi-threaded, static lib version of the C++ standard lib - -#----------------------------- Source Files ----------------------------------- - - -#Standalone executable -!if "$(MODELREF_TARGET_TYPE)" == "NONE" -PRODUCT = $(RELATIVE_PATH_TO_ANCHOR)\$(MODEL).exe -REQ_SRCS = $(MODULES) $(EXT_SRC) - -#Model Reference Target -!else -PRODUCT = $(MODELLIB) -REQ_SRCS = $(MODULES) $(EXT_SRC) -!endif - -USER_SRCS = - -SRCS = $(REQ_SRCS) $(USER_SRCS) $(S_FUNCTIONS) -OBJS_CPP_UPPER = $(SRCS:.CPP=.obj) -OBJS_CPP_LOWER = $(OBJS_CPP_UPPER:.cpp=.obj) -OBJS_C_UPPER = $(OBJS_CPP_LOWER:.C=.obj) -OBJS = $(OBJS_C_UPPER:.c=.obj) -SHARED_OBJS = $(SHARED_SRC:.c=.obj) -# ------------------------- Additional Libraries ------------------------------ - -LIBS = - - -LIBS = $(LIBS) - -# ---------------------------- Linker Script ---------------------------------- - -CMD_FILE = $(MODEL).lnk -GEN_LNK_SCRIPT = $(MATLAB_ROOT)\rtw\c\tools\mkvc_lnk.pl - -#--------------------------------- Rules -------------------------------------- -all: set_environment_variables $(PRODUCT) - -!if "$(MODELREF_TARGET_TYPE)" == "NONE" -#--- Stand-alone model --- -$(PRODUCT) : $(OBJS) $(SHARED_LIB) $(LIBS) $(MODELREF_LINK_LIBS) - @cmd /C "echo ### Linking ..." - $(PERL) $(GEN_LNK_SCRIPT) $(CMD_FILE) $(OBJS) - $(LD) $(LDFLAGS) $(S_FUNCTIONS_LIB) $(SHARED_LIB) $(LIBS) $(MAT_LIBS) @$(CMD_FILE) @$(MODELREF_LINK_RSPFILE) -out:$@ - @del $(CMD_FILE) - @cmd /C "echo $(BUILD_SUCCESS) executable $(MODEL).exe" -!else -#--- Model reference code generation Target --- -$(PRODUCT) : $(OBJS) $(SHARED_LIB) - @cmd /C "echo ### Linking ..." - $(PERL) $(GEN_LNK_SCRIPT) $(CMD_FILE) $(OBJS) - $(LD) -lib /OUT:$(PRODUCT) @$(CMD_FILE) $(S_FUNCTIONS_LIB) - @cmd /C "echo $(BUILD_SUCCESS) static library $(MODELLIB)" -!endif - -{$(MATLAB_ROOT)\rtw\c\src}.c.obj : - @cmd /C "echo ### Compiling $<" - $(CC) $(CFLAGS) $< - -{$(MATLAB_ROOT)\rtw\c\src\ext_mode\common}.c.obj : - @cmd /C "echo ### Compiling $<" - $(CC) $(CFLAGS) $< - -{$(MATLAB_ROOT)\rtw\c\src\rtiostream\rtiostreamtcpip}.c.obj : - @cmd /C "echo ### Compiling $<" - $(CC) $(CFLAGS) $< - -{$(MATLAB_ROOT)\rtw\c\src\ext_mode\serial}.c.obj : - @cmd /C "echo ### Compiling $<" - $(CC) $(CFLAGS) $< - -{$(MATLAB_ROOT)\rtw\c\src\ext_mode\custom}.c.obj : - @cmd /C "echo ### Compiling $<" - $(CC) $(CFLAGS) $< - -# Additional sources - - - - - -# Put these rule last, otherwise nmake will check toolboxes first - -{$(RELATIVE_PATH_TO_ANCHOR)}.c.obj : - @cmd /C "echo ### Compiling $<" - $(CC) $(CFLAGS) $< - -{$(RELATIVE_PATH_TO_ANCHOR)}.cpp.obj : - @cmd /C "echo ### Compiling $<" - $(CC) $(CPPFLAGS) $< - -.c.obj : - @cmd /C "echo ### Compiling $<" - $(CC) $(CFLAGS) $< - -.cpp.obj : - @cmd /C "echo ### Compiling $<" - $(CC) $(CPPFLAGS) $< - -!if "$(SHARED_LIB)" != "" -$(SHARED_LIB) : $(SHARED_SRC) - @cmd /C "echo ### Creating $@" - @$(CC) $(CFLAGS) -Fo$(SHARED_BIN_DIR)\ @<< -$? -<< - @$(LIBCMD) /nologo /out:$@ $(SHARED_OBJS) - @cmd /C "echo ### $@ Created" -!endif - - -set_environment_variables: - @set INCLUDE=$(INCLUDE) - @set LIB=$(LIB) - -# Libraries: - - - - - -#----------------------------- Dependencies ----------------------------------- - -$(OBJS) : $(MAKEFILE) rtw_proj.tmw diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c index eab8c7d6e0..458ddb9eb0 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter_terminate' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:24 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h index fafd866e42..a939eee200 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter_terminate' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:24 2012 * */ @@ -14,6 +14,8 @@ #include #include #include +#include "rt_defines.h" +#include "rt_nonfinite.h" #include "rtwtypes.h" #include "attitudeKalmanfilter_types.h" diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h index 05f4664cd9..599a168848 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:22 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/buildInfo.mat b/apps/attitude_estimator_ekf/codegen/buildInfo.mat deleted file mode 100755 index b811d003977e5a3d432f33c24d297eddea2b494b..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 4114 zcma)8X*3iJ*d4n`w(JZeZ%DSHv4tT~Dnvu}HA}X!4_SsmmTD~7vWFK+=f`uNd(V09J?C1P!fu*gmWODZm$x#7sr%gv_EnR= z73>w|2agCbkiTVi$I@EoqME!}gs)eW?_GI#sDb<)-@EeHql4uyT#!GnXP~WT0D;JB zoxh+Z|Nq3WAmD$&W@+_rT;~G-V#{Zo?je>&#bj0N*6NDZ7tp&4MQX~cx5w+&_1>sy z%JbT_UCU)Rv9w;S5dyjdpUr=c)eBsSvciyEFqL@ZG<1f1ELCy({J<>8uog^zuE#Uj zbBVed7ds>+w03%Q0Xi=gpXVV+Ie>`EiZBwMR=Lz#cZK|wqq0dXhxE_yaoZXW*(|L% zo&`{apt=JHg*hACFR1s;HB`S|8O6ha#^XmlP-crCoyfVnj%hbqkBKQOCX6{bEs0FY)~4p4if+?F=_$&;mIl&o61m=dW{?c_%?G zI@OIW=4#05i=%Hk0>T1^yiFp}Q-ZOYs)9$grdL#A9t5wD(d_j(F+)uBDx7DX6MH$$ z=hHp{^&0w$f6%(02L$Ad9~jhGl6rdy9P_ni@uwhy<|}E;Pvsuid-vlHy%$&Eu6%OT zOP^PrkZ-j&S~l4>QqBxmSj*Z$x7uzG_7~E+6j*afcn9yV*KfKsiHGu-jjD=A;kvZ8 z(<|C!I-af)q$o$K(TTSk%h zcJJ6C2gs=#Ijj)an(ZIw&ebs7;@ls8JSO_~^rMM@%nD*{gU_&fx4UImPqq`W?Z=E^ zMeWae{Pa2ZsP@C<%NpmhbiQ}0$rO1VlpO@uy0^PI1e6rk;NYn6pgh!R>9y_mZ~gRr z?2L7dOI%hc#$*VnxH#=Y1O?P&pO-mQE;gZ)U87!xZZ^ZKmcBj7R}T@WJ(Zc=_Qme@ zmhB=<4?HlKsi(AN1Xx#5WTH-i?bhZCY5a;w8Perr%IcM{Fsc4(qE@9HMTNb}p=CeT z_Wj3oMHDibxXyftQ|0uSbKU@-p=on^Oy+$=781cqw0-^h{Tv}iEhkHIptwr55Lr&F zlBX#H`xt7CjN6>7s04fH9sjjg^uxRyi&-R^0=+@m0y6bcETsYDLt>ROO@fzcjzVS; zJ(Os7c$xMnmd*f6VW8iFBEif2Lu;IbqlsJVuMEh#ysmrEnkqCac4RRYE=-H(?7K$n z0w!c($wD-(6oMipo-?Tm3l@53LF8he2liYea;FsY_S__@a)zFwnX!8)Qck4Y0WK?0 zq*LaB36HTM>$7N|WX4_mN6!H6z@kT;fl{4Un%qAXt(vaU}Vh9C6bsK2k36|9O&BW5kyfNkr%j z!Q~i}DTFd2Rh(8aMd=14wukkX5X>pv1>X=-^i3>WfYFjnm_6Cw^^B}WtKfpVQs=c8 z;?;0D#^wOofVN@B2&?GhNc0x?-X~xAYrnl)idws;L>f+bzRS0N#IebeyRnJPQ#?S* z?0cXXPGW8v;z%NG5Pm;sHs{4?NiD8YqG}1wYVQ^3C7m`1-AW zEn0JO)zfcOA%??1D|?bXrCo~=IE?`AQBTok2jJ>MQxE#ai3lkMh!UyIke)_x{nKke z3#p@7p$JFQ$^;oDu|8M)gyfKaqvKS>Zf3M9!7Y-ds0lu}YwW@H*s+#;S!TYKr^f}` z&9%CJWVqdT0%Ja{j@paKp5$3MMJm#Ya;fg-z+6%>XforA8n1S7U@T#~`FgsEngoek z63KhUq`mgLtoZVpcyN&TI3PY60`BOYN(<)Rd)JmwYI4;VLq(tNRBk@u0chgi-j#PJJED&jO1avcBunt58h;H0D-yt18{Ygc)>&8T65+ zNABnBwIXfAYiuWR8DvcBt6)4*Cl#YYv-fnXgM=^^*?V2pF3M5+MlwI?m!?C63{u8B zS@Gs9o}y1b!MA_2vmPlPym?7tajruZMvgnn98$ec|m(i-B~n z1>n_P&sj2)2LNEPc8XnW2G2`A5EeEsj~y1tjy0~4aF>F=JFPT2d_%LSM62xoYe8dpn;yF4GrwVvD6Kre;?f!Qz@$n(4i5%)4?^e;NM)@~jG%9arx*WgTK{ z2i^L&#)qEIqk!1R+3R8ps}4#%#=Anh!m>iL!WBXl!n0!Y9=4xu8+o`ZQTo{ay>hpB zHxz$f^g&nFX=bw0z+3AwotElT1P>o#eQ4RN)%7*&)=(w!HGwNnt?75@g5v z9q^1s`h4?}*ej#t*9>$#7S4N%BxEHrPm#}0R$5|o&+Z$STTq3?zFTUkxS8mHtB*a1A8b8U5bT!WT z8yJ(Pl~Kg0-P{E zNOZkqt4#x4^wr+6knO26l*{)!scWos|Z&dhktO_{foj=~?n6CaT$GRYX}Zoa*gBO+TYuUra_2wdQ@8IX(RuH;}pfd!bG znEqhj*!7saQ|Bn!sqixq%p3YptDP_H{nH~qBP8B{fG>76Hc1}`>hhaPwm-I{DTVJX zPOmLp%d~l?w{3%s&-**Itn1Rp+Mr_0q>SZxU^BqhF&>@QkFs~~?v)MSk91CWpLYNI z(!jT-o#Jn;5bJWS4OD+4zA1q)oko07iMP0J_Gz{yH}A!>fr#t^ z(nJ7*tt0w#^uMZ)0Q{@^>pq^IC4k{zlTU2JYA1!TCpNU%UrQ8-1J?3qmQm%y`7pxp z&P>Orav0l-o_712CbmiUZM|DiGPmC8jm}>f#(h>%8XLn-pTAIl1`1VH=~qzFhbngR zUKcMcz@5RtL<^I{5}YE^pJusRhDr}70qT|hyZn#-3;RF%S%(R0nF~TOm#;s}y@hVh zZ3NxtGPSX{G2vAVWM292tJ#_FJJzRk-Qzc+1Mm{}z_mG_airR)n>{i(VB0kE#_FHg z%`)f|8u3tQ-}gywbOLnH=}h}C>=!QoP~wh_q~OwPYpwUE;=B*Z*+qmJVIlaHsFNI{ zURj8_#CU;5O%bpbCtG&li;qHIQ8wBZ$ui1z=CC6jV(ogVcFDDi8;lYa(LD6Es{?=n zRNIoFlL^dMw6-ho!wOa_aBaUlV*cGpWsrj;kLeM};Lm8@hl!U3KB}TNQDy=^YwBUz zQU;kmMZ*@M`sio5ComBwYYb4H8p)$6RRhG2X%DYih}H&;B?l^{55;OnHq_SXi%DFt z!oc`VtJL@CO(zUNTM#0r<+A@TIL z@aZm@$J$UTs8?phx0mZG{Bz4*s;cAiLzlH!(uf!7EBdf$CVwSbunPU&?adbgi)(8O zhbxp_?ve?!V76yy|8Ar&@Si)+m0)r1Zn4_M8w~V+3C(KCN+o^;?KWW*oRgWroyTXZ zf2Ze;nM^-Q;KG=@{CK0m{f?m;?c`pzZpVz)`;XPDSFE{}%X0pNfKU9ramRo^(d1_B z=kmXW1%CxB@tV_6cTo{kI?zv_49GRk|&F|>}cuK2Qp}QbkgJq2D z=C9TcqDNL*EPv?Y0S4IhV~(7dSGb*FJhSuqzOXpHDJRh$FXx7Y9M}|D0{{f59RFymjO^0p8Z!yq@X%QYm`u z*HZx^1yfkk555{&@&bn?R7Cvv(KWi0l%wRrvjm}vyprPlCnqlOZ~_$Wa0&bm*2WhF diff --git a/apps/attitude_estimator_ekf/codegen/cross.c b/apps/attitude_estimator_ekf/codegen/cross.c new file mode 100755 index 0000000000..49f655ece8 --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/cross.c @@ -0,0 +1,37 @@ +/* + * cross.c + * + * Code generation for function 'cross' + * + * C source code generated on: Mon Sep 17 20:13:23 2012 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "attitudeKalmanfilter.h" +#include "cross.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ + +/* + * + */ +void cross(const real32_T a[3], const real32_T b[3], real32_T c[3]) +{ + c[0] = a[1] * b[2] - a[2] * b[1]; + c[1] = a[2] * b[0] - a[0] * b[2]; + c[2] = a[0] * b[1] - a[1] * b[0]; +} + +/* End of code generation (cross.c) */ diff --git a/apps/attitude_estimator_ekf/codegen/cross.h b/apps/attitude_estimator_ekf/codegen/cross.h new file mode 100755 index 0000000000..844d8138fd --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/cross.h @@ -0,0 +1,34 @@ +/* + * cross.h + * + * Code generation for function 'cross' + * + * C source code generated on: Mon Sep 17 20:13:23 2012 + * + */ + +#ifndef __CROSS_H__ +#define __CROSS_H__ +/* Include files */ +#include +#include +#include +#include +#include "rt_defines.h" +#include "rt_nonfinite.h" + +#include "rtwtypes.h" +#include "attitudeKalmanfilter_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void cross(const real32_T a[3], const real32_T b[3], real32_T c[3]); +#endif +/* End of code generation (cross.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/diag.c b/apps/attitude_estimator_ekf/codegen/diag.c new file mode 100755 index 0000000000..e54d52a384 --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/diag.c @@ -0,0 +1,42 @@ +/* + * diag.c + * + * Code generation for function 'diag' + * + * C source code generated on: Mon Sep 17 20:13:22 2012 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "attitudeKalmanfilter.h" +#include "diag.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ + +/* + * + */ +void diag(const real32_T v[3], real32_T d[9]) +{ + int32_T j; + for (j = 0; j < 9; j++) { + d[j] = 0.0F; + } + + for (j = 0; j < 3; j++) { + d[j + 3 * j] = v[j]; + } +} + +/* End of code generation (diag.c) */ diff --git a/apps/attitude_estimator_ekf/codegen/diag.h b/apps/attitude_estimator_ekf/codegen/diag.h new file mode 100755 index 0000000000..a4a2a4c829 --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/diag.h @@ -0,0 +1,34 @@ +/* + * diag.h + * + * Code generation for function 'diag' + * + * C source code generated on: Mon Sep 17 20:13:23 2012 + * + */ + +#ifndef __DIAG_H__ +#define __DIAG_H__ +/* Include files */ +#include +#include +#include +#include +#include "rt_defines.h" +#include "rt_nonfinite.h" + +#include "rtwtypes.h" +#include "attitudeKalmanfilter_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void diag(const real32_T v[3], real32_T d[9]); +#endif +/* End of code generation (diag.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/eye.c b/apps/attitude_estimator_ekf/codegen/eye.c index e4655839c4..aece401c25 100755 --- a/apps/attitude_estimator_ekf/codegen/eye.c +++ b/apps/attitude_estimator_ekf/codegen/eye.c @@ -3,7 +3,7 @@ * * Code generation for function 'eye' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:23 2012 * */ @@ -27,12 +27,12 @@ /* * */ -void b_eye(real_T I[144]) +void b_eye(real_T I[81]) { int32_T i; - memset((void *)&I[0], 0, 144U * sizeof(real_T)); - for (i = 0; i < 12; i++) { - I[i + 12 * i] = 1.0; + memset((void *)&I[0], 0, 81U * sizeof(real_T)); + for (i = 0; i < 9; i++) { + I[i + 9 * i] = 1.0; } } diff --git a/apps/attitude_estimator_ekf/codegen/eye.h b/apps/attitude_estimator_ekf/codegen/eye.h index e8881747f6..e715ae1c39 100755 --- a/apps/attitude_estimator_ekf/codegen/eye.h +++ b/apps/attitude_estimator_ekf/codegen/eye.h @@ -3,7 +3,7 @@ * * Code generation for function 'eye' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:23 2012 * */ @@ -14,6 +14,8 @@ #include #include #include +#include "rt_defines.h" +#include "rt_nonfinite.h" #include "rtwtypes.h" #include "attitudeKalmanfilter_types.h" @@ -27,7 +29,7 @@ /* Variable Definitions */ /* Function Declarations */ -extern void b_eye(real_T I[144]); +extern void b_eye(real_T I[81]); extern void eye(real_T I[9]); #endif /* End of code generation (eye.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/find.c b/apps/attitude_estimator_ekf/codegen/find.c new file mode 100755 index 0000000000..532ba43970 --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/find.c @@ -0,0 +1,77 @@ +/* + * find.c + * + * Code generation for function 'find' + * + * C source code generated on: Mon Sep 17 20:13:22 2012 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "attitudeKalmanfilter.h" +#include "find.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ + +/* + * + */ +void find(const int8_T x[9], real_T i_data[9], int32_T i_sizes[1]) +{ + int32_T idx; + int32_T ii; + boolean_T exitg1; + boolean_T guard1 = FALSE; + int32_T i2; + int8_T b_i_data[9]; + idx = 0; + i_sizes[0] = 9; + ii = 1; + exitg1 = 0U; + while ((exitg1 == 0U) && (ii <= 9)) { + guard1 = FALSE; + if (x[ii - 1] != 0) { + idx++; + i_data[idx - 1] = (real_T)ii; + if (idx >= 9) { + exitg1 = 1U; + } else { + guard1 = TRUE; + } + } else { + guard1 = TRUE; + } + + if (guard1 == TRUE) { + ii++; + } + } + + if (1 > idx) { + idx = 0; + } + + ii = idx - 1; + for (i2 = 0; i2 <= ii; i2++) { + b_i_data[i2] = (int8_T)i_data[i2]; + } + + i_sizes[0] = idx; + ii = idx - 1; + for (i2 = 0; i2 <= ii; i2++) { + i_data[i2] = (real_T)b_i_data[i2]; + } +} + +/* End of code generation (find.c) */ diff --git a/apps/attitude_estimator_ekf/codegen/find.h b/apps/attitude_estimator_ekf/codegen/find.h new file mode 100755 index 0000000000..ceb90b6cf6 --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/find.h @@ -0,0 +1,34 @@ +/* + * find.h + * + * Code generation for function 'find' + * + * C source code generated on: Mon Sep 17 20:13:22 2012 + * + */ + +#ifndef __FIND_H__ +#define __FIND_H__ +/* Include files */ +#include +#include +#include +#include +#include "rt_defines.h" +#include "rt_nonfinite.h" + +#include "rtwtypes.h" +#include "attitudeKalmanfilter_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void find(const int8_T x[9], real_T i_data[9], int32_T i_sizes[1]); +#endif +/* End of code generation (find.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/mrdivide.c b/apps/attitude_estimator_ekf/codegen/mrdivide.c index cb81b53282..2fcaf8cb67 100755 --- a/apps/attitude_estimator_ekf/codegen/mrdivide.c +++ b/apps/attitude_estimator_ekf/codegen/mrdivide.c @@ -3,7 +3,7 @@ * * Code generation for function 'mrdivide' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:23 2012 * */ @@ -21,136 +21,719 @@ /* Variable Definitions */ /* Function Declarations */ +static real32_T b_eml_matlab_zlarfg(real32_T *alpha1, const real32_T *x); +static void eml_lusolve(const real32_T A_data[81], const int32_T A_sizes[2], + real32_T B_data[81], int32_T B_sizes[2]); +static real32_T eml_matlab_zlarfg(int32_T n, real32_T *alpha1, real32_T x_data + [81], int32_T x_sizes[2], int32_T ix0); +static void eml_qrsolve(const real32_T A_data[81], const int32_T A_sizes[2], + real32_T B_data[81], int32_T B_sizes[2], real32_T Y_data[81], int32_T Y_sizes + [2]); +static real32_T eml_xnrm2(int32_T n, const real32_T x_data[81], const int32_T + x_sizes[2], int32_T ix0); /* Function Definitions */ /* * */ -void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108]) +static real32_T b_eml_matlab_zlarfg(real32_T *alpha1, const real32_T *x) { - int32_T jy; + return 0.0F; +} + +/* + * + */ +static void eml_lusolve(const real32_T A_data[81], const int32_T A_sizes[2], + real32_T B_data[81], int32_T B_sizes[2]) +{ + int32_T loop_ub; int32_T iy; - real32_T b_A[81]; - int8_T ipiv[9]; + real32_T b_A_data[81]; + int32_T jA; + int32_T tmp_data[9]; + int32_T ipiv_data[9]; + int32_T ldap1; int32_T j; int32_T mmj; int32_T jj; - int32_T jp1j; - int32_T c; int32_T ix; real32_T temp; int32_T k; real32_T s; - int32_T loop_ub; - real32_T Y[108]; - for (jy = 0; jy < 9; jy++) { - for (iy = 0; iy < 9; iy++) { - b_A[iy + 9 * jy] = B[jy + 9 * iy]; - } - - ipiv[jy] = (int8_T)(1 + jy); + int32_T jrow; + int32_T b_loop_ub; + int32_T c; + loop_ub = A_sizes[0] * A_sizes[1] - 1; + for (iy = 0; iy <= loop_ub; iy++) { + b_A_data[iy] = A_data[iy]; } - for (j = 0; j < 8; j++) { - mmj = -j; - jj = j * 10; - jp1j = jj + 1; - c = mmj + 9; - jy = 0; - ix = jj; - temp = fabsf(b_A[jj]); - for (k = 2; k <= c; k++) { - ix++; - s = fabsf(b_A[ix]); - if (s > temp) { - jy = k - 1; - temp = s; + iy = A_sizes[1]; + jA = A_sizes[1]; + jA = iy <= jA ? iy : jA; + if (jA < 1) { + } else { + loop_ub = jA - 1; + for (iy = 0; iy <= loop_ub; iy++) { + tmp_data[iy] = 1 + iy; + } + + loop_ub = jA - 1; + for (iy = 0; iy <= loop_ub; iy++) { + ipiv_data[iy] = tmp_data[iy]; + } + } + + ldap1 = A_sizes[1] + 1; + iy = A_sizes[1] - 1; + jA = A_sizes[1]; + loop_ub = iy <= jA ? iy : jA; + for (j = 1; j <= loop_ub; j++) { + mmj = (A_sizes[1] - j) + 1; + jj = (j - 1) * ldap1; + if (mmj < 1) { + jA = -1; + } else { + jA = 0; + if (mmj > 1) { + ix = jj; + temp = (real32_T)fabs(b_A_data[jj]); + for (k = 1; k + 1 <= mmj; k++) { + ix++; + s = (real32_T)fabs(b_A_data[ix]); + if (s > temp) { + jA = k; + temp = s; + } + } } } - if ((real_T)b_A[jj + jy] != 0.0) { - if (jy != 0) { - ipiv[j] = (int8_T)((j + jy) + 1); - ix = j; - iy = j + jy; - for (k = 0; k < 9; k++) { - temp = b_A[ix]; - b_A[ix] = b_A[iy]; - b_A[iy] = temp; - ix += 9; - iy += 9; + if ((real_T)b_A_data[jj + jA] != 0.0) { + if (jA != 0) { + ipiv_data[j - 1] = j + jA; + jrow = j - 1; + iy = jrow + jA; + for (k = 1; k <= A_sizes[1]; k++) { + temp = b_A_data[jrow]; + b_A_data[jrow] = b_A_data[iy]; + b_A_data[iy] = temp; + jrow += A_sizes[1]; + iy += A_sizes[1]; } } - loop_ub = (jp1j + mmj) + 8; - for (iy = jp1j; iy + 1 <= loop_ub; iy++) { - b_A[iy] /= b_A[jj]; + b_loop_ub = jj + mmj; + for (jA = jj + 1; jA + 1 <= b_loop_ub; jA++) { + b_A_data[jA] /= b_A_data[jj]; } } - c = 8 - j; - jy = jj + 9; - for (iy = 1; iy <= c; iy++) { - if ((real_T)b_A[jy] != 0.0) { - temp = b_A[jy] * -1.0F; - ix = jp1j; - loop_ub = (mmj + jj) + 18; - for (k = 10 + jj; k + 1 <= loop_ub; k++) { - b_A[k] += b_A[ix] * temp; + c = A_sizes[1] - j; + jA = jj + ldap1; + iy = jj + A_sizes[1]; + for (jrow = 1; jrow <= c; jrow++) { + if ((real_T)b_A_data[iy] != 0.0) { + temp = b_A_data[iy] * -1.0F; + ix = jj; + b_loop_ub = (mmj + jA) - 1; + for (k = jA; k + 1 <= b_loop_ub; k++) { + b_A_data[k] += b_A_data[ix + 1] * temp; ix++; } } - jy += 9; - jj += 9; + iy += A_sizes[1]; + jA += A_sizes[1]; } } - for (jy = 0; jy < 12; jy++) { - for (iy = 0; iy < 9; iy++) { - Y[iy + 9 * jy] = A[jy + 12 * iy]; - } - } - - for (iy = 0; iy < 9; iy++) { - if (ipiv[iy] != iy + 1) { - for (j = 0; j < 12; j++) { - temp = Y[iy + 9 * j]; - Y[iy + 9 * j] = Y[(ipiv[iy] + 9 * j) - 1]; - Y[(ipiv[iy] + 9 * j) - 1] = temp; + for (jA = 0; jA + 1 <= A_sizes[1]; jA++) { + if (ipiv_data[jA] != jA + 1) { + for (j = 0; j < 9; j++) { + temp = B_data[jA + B_sizes[0] * j]; + B_data[jA + B_sizes[0] * j] = B_data[(ipiv_data[jA] + B_sizes[0] * j) - + 1]; + B_data[(ipiv_data[jA] + B_sizes[0] * j) - 1] = temp; } } } - for (j = 0; j < 12; j++) { - c = 9 * j; - for (k = 0; k < 9; k++) { - jy = 9 * k; - if ((real_T)Y[k + c] != 0.0) { - for (iy = k + 2; iy < 10; iy++) { - Y[(iy + c) - 1] -= Y[k + c] * b_A[(iy + jy) - 1]; + if (B_sizes[0] == 0) { + } else { + for (j = 0; j < 9; j++) { + c = A_sizes[1] * j; + for (k = 0; k + 1 <= A_sizes[1]; k++) { + iy = A_sizes[1] * k; + if ((real_T)B_data[k + c] != 0.0) { + for (jA = k + 1; jA + 1 <= A_sizes[1]; jA++) { + B_data[jA + c] -= B_data[k + c] * b_A_data[jA + iy]; + } } } } } - for (j = 0; j < 12; j++) { - c = 9 * j; - for (k = 8; k > -1; k += -1) { - jy = 9 * k; - if ((real_T)Y[k + c] != 0.0) { - Y[k + c] /= b_A[k + jy]; - for (iy = 0; iy + 1 <= k; iy++) { - Y[iy + c] -= Y[k + c] * b_A[iy + jy]; + if (B_sizes[0] == 0) { + } else { + for (j = 0; j < 9; j++) { + c = A_sizes[1] * j; + for (k = A_sizes[1] - 1; k + 1 > 0; k--) { + iy = A_sizes[1] * k; + if ((real_T)B_data[k + c] != 0.0) { + B_data[k + c] /= b_A_data[k + iy]; + for (jA = 0; jA + 1 <= k; jA++) { + B_data[jA + c] -= B_data[k + c] * b_A_data[jA + iy]; + } + } + } + } + } +} + +/* + * + */ +static real32_T eml_matlab_zlarfg(int32_T n, real32_T *alpha1, real32_T x_data + [81], int32_T x_sizes[2], int32_T ix0) +{ + real32_T tau; + int32_T nm1; + real32_T xnorm; + real32_T a; + int32_T knt; + int32_T loop_ub; + int32_T k; + tau = 0.0F; + if (n <= 0) { + } else { + nm1 = n - 2; + xnorm = eml_xnrm2(nm1 + 1, x_data, x_sizes, ix0); + if ((real_T)xnorm != 0.0) { + a = (real32_T)fabs(*alpha1); + xnorm = (real32_T)fabs(xnorm); + if (a < xnorm) { + a /= xnorm; + xnorm *= (real32_T)sqrt(a * a + 1.0F); + } else if (a > xnorm) { + xnorm /= a; + xnorm = (real32_T)sqrt(xnorm * xnorm + 1.0F) * a; + } else if (rtIsNaNF(xnorm)) { + } else { + xnorm = a * 1.41421354F; + } + + if ((real_T)*alpha1 >= 0.0) { + xnorm = -xnorm; + } + + if ((real32_T)fabs(xnorm) < 9.86076132E-32F) { + knt = 0; + do { + knt++; + loop_ub = ix0 + nm1; + for (k = ix0; k <= loop_ub; k++) { + x_data[k - 1] *= 1.01412048E+31F; + } + + xnorm *= 1.01412048E+31F; + *alpha1 *= 1.01412048E+31F; + } while (!((real32_T)fabs(xnorm) >= 9.86076132E-32F)); + + xnorm = eml_xnrm2(nm1 + 1, x_data, x_sizes, ix0); + a = (real32_T)fabs(*alpha1); + xnorm = (real32_T)fabs(xnorm); + if (a < xnorm) { + a /= xnorm; + xnorm *= (real32_T)sqrt(a * a + 1.0F); + } else if (a > xnorm) { + xnorm /= a; + xnorm = (real32_T)sqrt(xnorm * xnorm + 1.0F) * a; + } else if (rtIsNaNF(xnorm)) { + } else { + xnorm = a * 1.41421354F; + } + + if ((real_T)*alpha1 >= 0.0) { + xnorm = -xnorm; + } + + tau = (xnorm - *alpha1) / xnorm; + *alpha1 = 1.0F / (*alpha1 - xnorm); + loop_ub = ix0 + nm1; + for (k = ix0; k <= loop_ub; k++) { + x_data[k - 1] *= *alpha1; + } + + for (k = 1; k <= knt; k++) { + xnorm *= 9.86076132E-32F; + } + + *alpha1 = xnorm; + } else { + tau = (xnorm - *alpha1) / xnorm; + *alpha1 = 1.0F / (*alpha1 - xnorm); + loop_ub = ix0 + nm1; + for (k = ix0; k <= loop_ub; k++) { + x_data[k - 1] *= *alpha1; + } + + *alpha1 = xnorm; + } + } + } + + return tau; +} + +/* + * + */ +static void eml_qrsolve(const real32_T A_data[81], const int32_T A_sizes[2], + real32_T B_data[81], int32_T B_sizes[2], real32_T Y_data[81], int32_T Y_sizes + [2]) +{ + real_T rankR; + real_T u1; + int32_T mn; + int32_T b_A_sizes[2]; + int32_T loop_ub; + int32_T k; + real32_T b_A_data[81]; + int32_T pvt; + int32_T b_mn; + int32_T tmp_data[9]; + int32_T jpvt_data[9]; + int8_T unnamed_idx_0; + real32_T work_data[9]; + int32_T nmi; + real32_T vn1_data[9]; + real32_T vn2_data[9]; + int32_T i; + int32_T i_i; + int32_T mmi; + int32_T ix; + real32_T smax; + real32_T s; + int32_T iy; + real32_T atmp; + real32_T tau_data[9]; + int32_T i_ip1; + int32_T lastv; + int32_T lastc; + boolean_T exitg2; + int32_T exitg1; + boolean_T firstNonZero; + real32_T t; + rankR = (real_T)A_sizes[0]; + u1 = (real_T)A_sizes[1]; + rankR = rankR <= u1 ? rankR : u1; + mn = (int32_T)rankR; + b_A_sizes[0] = A_sizes[0]; + b_A_sizes[1] = A_sizes[1]; + loop_ub = A_sizes[0] * A_sizes[1] - 1; + for (k = 0; k <= loop_ub; k++) { + b_A_data[k] = A_data[k]; + } + + k = A_sizes[0]; + pvt = A_sizes[1]; + b_mn = k <= pvt ? k : pvt; + if (A_sizes[1] < 1) { + } else { + loop_ub = A_sizes[1] - 1; + for (k = 0; k <= loop_ub; k++) { + tmp_data[k] = 1 + k; + } + + loop_ub = A_sizes[1] - 1; + for (k = 0; k <= loop_ub; k++) { + jpvt_data[k] = tmp_data[k]; + } + } + + if ((A_sizes[0] == 0) || (A_sizes[1] == 0)) { + } else { + unnamed_idx_0 = (int8_T)A_sizes[1]; + loop_ub = unnamed_idx_0 - 1; + for (k = 0; k <= loop_ub; k++) { + work_data[k] = 0.0F; + } + + k = 1; + for (nmi = 0; nmi + 1 <= A_sizes[1]; nmi++) { + vn1_data[nmi] = eml_xnrm2(A_sizes[0], A_data, A_sizes, k); + vn2_data[nmi] = vn1_data[nmi]; + k += A_sizes[0]; + } + + for (i = 0; i + 1 <= b_mn; i++) { + i_i = i + i * A_sizes[0]; + nmi = A_sizes[1] - i; + mmi = (A_sizes[0] - i) - 1; + if (nmi < 1) { + pvt = -1; + } else { + pvt = 0; + if (nmi > 1) { + ix = i; + smax = (real32_T)fabs(vn1_data[i]); + for (k = 1; k + 1 <= nmi; k++) { + ix++; + s = (real32_T)fabs(vn1_data[ix]); + if (s > smax) { + pvt = k; + smax = s; + } + } + } + } + + pvt += i; + if (pvt + 1 != i + 1) { + ix = A_sizes[0] * pvt; + iy = A_sizes[0] * i; + for (k = 1; k <= A_sizes[0]; k++) { + s = b_A_data[ix]; + b_A_data[ix] = b_A_data[iy]; + b_A_data[iy] = s; + ix++; + iy++; + } + + k = jpvt_data[pvt]; + jpvt_data[pvt] = jpvt_data[i]; + jpvt_data[i] = k; + vn1_data[pvt] = vn1_data[i]; + vn2_data[pvt] = vn2_data[i]; + } + + if (i + 1 < A_sizes[0]) { + atmp = b_A_data[i_i]; + smax = eml_matlab_zlarfg(mmi + 1, &atmp, b_A_data, b_A_sizes, i_i + 2); + tau_data[i] = smax; + } else { + atmp = b_A_data[i_i]; + smax = b_A_data[i_i]; + s = b_eml_matlab_zlarfg(&atmp, &smax); + b_A_data[i_i] = smax; + tau_data[i] = s; + } + + b_A_data[i_i] = atmp; + if (i + 1 < A_sizes[1]) { + atmp = b_A_data[i_i]; + b_A_data[i_i] = 1.0F; + i_ip1 = (i + (i + 1) * A_sizes[0]) + 1; + if ((real_T)tau_data[i] != 0.0) { + lastv = mmi; + pvt = i_i + mmi; + while ((lastv + 1 > 0) && ((real_T)b_A_data[pvt] == 0.0)) { + lastv--; + pvt--; + } + + lastc = nmi - 1; + exitg2 = 0U; + while ((exitg2 == 0U) && (lastc > 0)) { + k = i_ip1 + (lastc - 1) * A_sizes[0]; + pvt = k + lastv; + do { + exitg1 = 0U; + if (k <= pvt) { + if ((real_T)b_A_data[k - 1] != 0.0) { + exitg1 = 1U; + } else { + k++; + } + } else { + lastc--; + exitg1 = 2U; + } + } while (exitg1 == 0U); + + if (exitg1 == 1U) { + exitg2 = 1U; + } + } + } else { + lastv = -1; + lastc = 0; + } + + if (lastv + 1 > 0) { + if (lastc == 0) { + } else { + for (iy = 1; iy <= lastc; iy++) { + work_data[iy - 1] = 0.0F; + } + + iy = 0; + loop_ub = i_ip1 + A_sizes[0] * (lastc - 1); + pvt = i_ip1; + while ((A_sizes[0] > 0) && (pvt <= loop_ub)) { + ix = i_i; + smax = 0.0F; + k = pvt + lastv; + for (nmi = pvt; nmi <= k; nmi++) { + smax += b_A_data[nmi - 1] * b_A_data[ix]; + ix++; + } + + work_data[iy] += smax; + iy++; + pvt += A_sizes[0]; + } + } + + smax = -tau_data[i]; + if ((real_T)smax == 0.0) { + } else { + pvt = 0; + for (nmi = 1; nmi <= lastc; nmi++) { + if ((real_T)work_data[pvt] != 0.0) { + s = work_data[pvt] * smax; + ix = i_i; + loop_ub = lastv + i_ip1; + for (k = i_ip1 - 1; k + 1 <= loop_ub; k++) { + b_A_data[k] += b_A_data[ix] * s; + ix++; + } + } + + pvt++; + i_ip1 += A_sizes[0]; + } + } + } + + b_A_data[i_i] = atmp; + } + + for (nmi = i + 1; nmi + 1 <= A_sizes[1]; nmi++) { + if ((real_T)vn1_data[nmi] != 0.0) { + smax = (real32_T)fabs(b_A_data[i + b_A_sizes[0] * nmi]) / vn1_data[nmi]; + smax = 1.0F - smax * smax; + if ((real_T)smax < 0.0) { + smax = 0.0F; + } + + s = vn1_data[nmi] / vn2_data[nmi]; + if (smax * (s * s) <= 0.000345266977F) { + if (i + 1 < A_sizes[0]) { + k = (i + A_sizes[0] * nmi) + 1; + smax = 0.0F; + if (mmi < 1) { + } else if (mmi == 1) { + smax = (real32_T)fabs(b_A_data[k]); + } else { + s = 0.0F; + firstNonZero = TRUE; + pvt = k + mmi; + while (k + 1 <= pvt) { + if (b_A_data[k] != 0.0F) { + atmp = (real32_T)fabs(b_A_data[k]); + if (firstNonZero) { + s = atmp; + smax = 1.0F; + firstNonZero = FALSE; + } else if (s < atmp) { + t = s / atmp; + smax = 1.0F + smax * t * t; + s = atmp; + } else { + t = atmp / s; + smax += t * t; + } + } + + k++; + } + + smax = s * (real32_T)sqrt(smax); + } + + vn1_data[nmi] = smax; + vn2_data[nmi] = vn1_data[nmi]; + } else { + vn1_data[nmi] = 0.0F; + vn2_data[nmi] = 0.0F; + } + } else { + vn1_data[nmi] *= (real32_T)sqrt(smax); + } } } } } - for (jy = 0; jy < 9; jy++) { - for (iy = 0; iy < 12; iy++) { - y[iy + 12 * jy] = Y[jy + 9 * iy]; + rankR = (real_T)A_sizes[0]; + u1 = (real_T)A_sizes[1]; + rankR = rankR >= u1 ? rankR : u1; + smax = (real32_T)rankR * (real32_T)fabs(b_A_data[0]) * 1.1920929E-7F; + rankR = 0.0; + k = 0; + while ((k + 1 <= mn) && (!((real32_T)fabs(b_A_data[k + b_A_sizes[0] * k]) <= + smax))) { + rankR++; + k++; + } + + unnamed_idx_0 = (int8_T)A_sizes[1]; + Y_sizes[0] = (int32_T)unnamed_idx_0; + Y_sizes[1] = 9; + loop_ub = unnamed_idx_0 * 9 - 1; + for (k = 0; k <= loop_ub; k++) { + Y_data[k] = 0.0F; + } + + for (nmi = 0; nmi + 1 <= mn; nmi++) { + if ((real_T)tau_data[nmi] != 0.0) { + for (k = 0; k < 9; k++) { + smax = B_data[nmi + B_sizes[0] * k]; + for (i = nmi + 1; i + 1 <= A_sizes[0]; i++) { + smax += b_A_data[i + b_A_sizes[0] * nmi] * B_data[i + B_sizes[0] * k]; + } + + smax *= tau_data[nmi]; + if ((real_T)smax != 0.0) { + B_data[nmi + B_sizes[0] * k] -= smax; + for (i = nmi + 1; i + 1 <= A_sizes[0]; i++) { + B_data[i + B_sizes[0] * k] -= b_A_data[i + b_A_sizes[0] * nmi] * + smax; + } + } + } + } + } + + for (k = 0; k < 9; k++) { + for (i = 0; (real_T)(i + 1) <= rankR; i++) { + Y_data[(jpvt_data[i] + Y_sizes[0] * k) - 1] = B_data[i + B_sizes[0] * k]; + } + + for (nmi = (int32_T)rankR - 1; nmi + 1 >= 1; nmi--) { + Y_data[(jpvt_data[nmi] + Y_sizes[0] * k) - 1] /= b_A_data[nmi + b_A_sizes + [0] * nmi]; + for (i = 0; i + 1 <= nmi; i++) { + Y_data[(jpvt_data[i] + Y_sizes[0] * k) - 1] -= Y_data[(jpvt_data[nmi] + + Y_sizes[0] * k) - 1] * b_A_data[i + b_A_sizes[0] * nmi]; + } + } + } +} + +/* + * + */ +static real32_T eml_xnrm2(int32_T n, const real32_T x_data[81], const int32_T + x_sizes[2], int32_T ix0) +{ + real32_T y; + real32_T scale; + boolean_T firstNonZero; + int32_T kend; + int32_T k; + real32_T absxk; + real32_T t; + y = 0.0F; + if (n < 1) { + } else if (n == 1) { + y = (real32_T)fabs(x_data[ix0 - 1]); + } else { + scale = 0.0F; + firstNonZero = TRUE; + kend = (ix0 + n) - 1; + for (k = ix0 - 1; k + 1 <= kend; k++) { + if (x_data[k] != 0.0F) { + absxk = (real32_T)fabs(x_data[k]); + if (firstNonZero) { + scale = absxk; + y = 1.0F; + firstNonZero = FALSE; + } else if (scale < absxk) { + t = scale / absxk; + y = 1.0F + y * t * t; + scale = absxk; + } else { + t = absxk / scale; + y += t * t; + } + } + } + + y = scale * (real32_T)sqrt(y); + } + + return y; +} + +/* + * + */ +void mrdivide(const real32_T A_data[81], const int32_T A_sizes[2], const + real32_T B_data[81], const int32_T B_sizes[2], real32_T y_data[81], + int32_T y_sizes[2]) +{ + int32_T b_A_sizes[2]; + int32_T loop_ub; + int32_T i3; + int32_T b_loop_ub; + int32_T i4; + real32_T b_A_data[81]; + int32_T b_B_sizes[2]; + real32_T b_B_data[81]; + int8_T unnamed_idx_0; + int32_T c_B_sizes[2]; + real32_T c_B_data[81]; + b_A_sizes[0] = B_sizes[1]; + b_A_sizes[1] = B_sizes[0]; + loop_ub = B_sizes[0] - 1; + for (i3 = 0; i3 <= loop_ub; i3++) { + b_loop_ub = B_sizes[1] - 1; + for (i4 = 0; i4 <= b_loop_ub; i4++) { + b_A_data[i4 + b_A_sizes[0] * i3] = B_data[i3 + B_sizes[0] * i4]; + } + } + + b_B_sizes[0] = A_sizes[1]; + b_B_sizes[1] = 9; + for (i3 = 0; i3 < 9; i3++) { + loop_ub = A_sizes[1] - 1; + for (i4 = 0; i4 <= loop_ub; i4++) { + b_B_data[i4 + b_B_sizes[0] * i3] = A_data[i3 + A_sizes[0] * i4]; + } + } + + if ((b_A_sizes[0] == 0) || (b_A_sizes[1] == 0) || (b_B_sizes[0] == 0)) { + unnamed_idx_0 = (int8_T)b_A_sizes[1]; + b_B_sizes[0] = (int32_T)unnamed_idx_0; + b_B_sizes[1] = 9; + loop_ub = unnamed_idx_0 * 9 - 1; + for (i3 = 0; i3 <= loop_ub; i3++) { + b_B_data[i3] = 0.0F; + } + } else if (b_A_sizes[0] == b_A_sizes[1]) { + eml_lusolve(b_A_data, b_A_sizes, b_B_data, b_B_sizes); + } else { + c_B_sizes[0] = b_B_sizes[0]; + c_B_sizes[1] = 9; + loop_ub = b_B_sizes[0] * 9 - 1; + for (i3 = 0; i3 <= loop_ub; i3++) { + c_B_data[i3] = b_B_data[i3]; + } + + eml_qrsolve(b_A_data, b_A_sizes, c_B_data, c_B_sizes, b_B_data, b_B_sizes); + } + + y_sizes[0] = 9; + y_sizes[1] = b_B_sizes[0]; + loop_ub = b_B_sizes[0] - 1; + for (i3 = 0; i3 <= loop_ub; i3++) { + for (i4 = 0; i4 < 9; i4++) { + y_data[i4 + 9 * i3] = b_B_data[i3 + b_B_sizes[0] * i4]; } } } diff --git a/apps/attitude_estimator_ekf/codegen/mrdivide.h b/apps/attitude_estimator_ekf/codegen/mrdivide.h index e81bfffc0a..a58faaa262 100755 --- a/apps/attitude_estimator_ekf/codegen/mrdivide.h +++ b/apps/attitude_estimator_ekf/codegen/mrdivide.h @@ -3,7 +3,7 @@ * * Code generation for function 'mrdivide' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:23 2012 * */ @@ -14,6 +14,8 @@ #include #include #include +#include "rt_defines.h" +#include "rt_nonfinite.h" #include "rtwtypes.h" #include "attitudeKalmanfilter_types.h" @@ -27,6 +29,6 @@ /* Variable Definitions */ /* Function Declarations */ -extern void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108]); +extern void mrdivide(const real32_T A_data[81], const int32_T A_sizes[2], const real32_T B_data[81], const int32_T B_sizes[2], real32_T y_data[81], int32_T y_sizes[2]); #endif /* End of code generation (mrdivide.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/norm.c b/apps/attitude_estimator_ekf/codegen/norm.c index 1fbde052bc..8d8e4e1109 100755 --- a/apps/attitude_estimator_ekf/codegen/norm.c +++ b/apps/attitude_estimator_ekf/codegen/norm.c @@ -3,7 +3,7 @@ * * Code generation for function 'norm' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:23 2012 * */ @@ -40,7 +40,7 @@ real32_T norm(const real32_T x[3]) firstNonZero = TRUE; for (k = 0; k < 3; k++) { if (x[k] != 0.0F) { - absxk = fabsf(x[k]); + absxk = (real32_T)fabs(x[k]); if (firstNonZero) { scale = absxk; y = 1.0F; @@ -56,7 +56,7 @@ real32_T norm(const real32_T x[3]) } } - return scale * sqrtf(y); + return scale * (real32_T)sqrt(y); } /* End of code generation (norm.c) */ diff --git a/apps/attitude_estimator_ekf/codegen/norm.h b/apps/attitude_estimator_ekf/codegen/norm.h index b0c7fe430e..479503ae36 100755 --- a/apps/attitude_estimator_ekf/codegen/norm.h +++ b/apps/attitude_estimator_ekf/codegen/norm.h @@ -3,7 +3,7 @@ * * Code generation for function 'norm' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:23 2012 * */ @@ -14,6 +14,8 @@ #include #include #include +#include "rt_defines.h" +#include "rt_nonfinite.h" #include "rtwtypes.h" #include "attitudeKalmanfilter_types.h" diff --git a/apps/attitude_estimator_ekf/codegen/rtGetInf.c b/apps/attitude_estimator_ekf/codegen/rtGetInf.c index 3cef176848..42b3af501f 100755 --- a/apps/attitude_estimator_ekf/codegen/rtGetInf.c +++ b/apps/attitude_estimator_ekf/codegen/rtGetInf.c @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:24 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rtGetInf.h b/apps/attitude_estimator_ekf/codegen/rtGetInf.h index ab2d5a70d5..20d8c7f058 100755 --- a/apps/attitude_estimator_ekf/codegen/rtGetInf.h +++ b/apps/attitude_estimator_ekf/codegen/rtGetInf.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:24 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rtGetNaN.c b/apps/attitude_estimator_ekf/codegen/rtGetNaN.c index 17a093461f..d29aea34b6 100755 --- a/apps/attitude_estimator_ekf/codegen/rtGetNaN.c +++ b/apps/attitude_estimator_ekf/codegen/rtGetNaN.c @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:24 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rtGetNaN.h b/apps/attitude_estimator_ekf/codegen/rtGetNaN.h index 2c254bbbf6..a14b6170b8 100755 --- a/apps/attitude_estimator_ekf/codegen/rtGetNaN.h +++ b/apps/attitude_estimator_ekf/codegen/rtGetNaN.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:24 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rt_defines.h b/apps/attitude_estimator_ekf/codegen/rt_defines.h new file mode 100755 index 0000000000..4cad63ada3 --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/rt_defines.h @@ -0,0 +1,24 @@ +/* + * rt_defines.h + * + * Code generation for function 'attitudeKalmanfilter' + * + * C source code generated on: Mon Sep 17 20:13:24 2012 + * + */ + +#ifndef __RT_DEFINES_H__ +#define __RT_DEFINES_H__ + +#include + +#define RT_PI 3.14159265358979323846 +#define RT_PIF 3.1415927F +#define RT_LN_10 2.30258509299404568402 +#define RT_LN_10F 2.3025851F +#define RT_LOG10E 0.43429448190325182765 +#define RT_LOG10EF 0.43429449F +#define RT_E 2.7182818284590452354 +#define RT_EF 2.7182817F +#endif +/* End of code generation (rt_defines.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c index 005c4f28de..bd04f52e66 100755 --- a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c +++ b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:24 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h index 6481f011f8..67b3ba205d 100755 --- a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h +++ b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:24 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rtw_proj.tmw b/apps/attitude_estimator_ekf/codegen/rtw_proj.tmw deleted file mode 100755 index 1fb585abd7..0000000000 --- a/apps/attitude_estimator_ekf/codegen/rtw_proj.tmw +++ /dev/null @@ -1 +0,0 @@ -Code generation project for attitudeKalmanfilter using C:\Program Files\MATLAB\R2011a\toolbox\coder\coder\rtw\c\xrt\xrt_vcx64.tmf. MATLAB root = C:\Program Files\MATLAB\R2011a. diff --git a/apps/attitude_estimator_ekf/codegen/rtwtypes.h b/apps/attitude_estimator_ekf/codegen/rtwtypes.h index fd629ebcd0..6feb2e1a99 100755 --- a/apps/attitude_estimator_ekf/codegen/rtwtypes.h +++ b/apps/attitude_estimator_ekf/codegen/rtwtypes.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:24 2012 * */ From dbd6cbea60ade756b10c693b905fb3a85329e185 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 19 Sep 2012 22:43:00 +0200 Subject: [PATCH 03/56] Minor cleanups, correct sensor scaling --- .../attitude_estimator_ekf_main.c | 74 +++++-------------- 1 file changed, 17 insertions(+), 57 deletions(-) diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index 6c18b044e5..58d415d86b 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -1,8 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2012 PX4 Development Team. All rights reserved. * Author: Tobias Naegeli - * Laurens Mackay * Lorenz Meier * * Redistribution and use in source and binary forms, with or without @@ -35,14 +34,11 @@ ****************************************************************************/ /* - * @file Extended Kalman Filter for Attitude Estimation + * @file attitude_estimator_ekf_main.c + * + * Extended Kalman Filter for Attitude Estimation. */ - -/**************************************************************************** - * Included Files - ****************************************************************************/ - #include #include #include @@ -114,9 +110,6 @@ static float Rot_matrix[9] = {1.f, 0, 0, 0, 0, 1.f }; /**< init: identity matrix */ -// static float x_aposteriori_k[12] = {0}; -// static float P_aposteriori_k[144] = {0}; - /* * [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) */ @@ -171,7 +164,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) /* */ /* update successful, copy data on every 2nd run and execute filter */ - } else if (loopcounter & 0x01) { + } else { orb_copy(ORB_ID(sensor_combined), sub_raw, &raw); @@ -179,24 +172,19 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) dt = (raw.timestamp - last_measurement) / 1000000.0f; last_measurement = raw.timestamp; - // XXX Read out accel range via SPI on init, assuming 4G range at 14 bit res here - float range_g = 4.0f; - float mag_offset[3] = {0}; /* scale from 14 bit to m/s2 */ - z_k[3] = ((raw.accelerometer_raw[0] * range_g) / 8192.0f) / 9.81f; // = accel * (1 / 32768.0f / 8.0f * 9.81f); - z_k[4] = ((raw.accelerometer_raw[1] * range_g) / 8192.0f) / 9.81f; // = accel * (1 / 32768.0f / 8.0f * 9.81f); - z_k[5] = ((raw.accelerometer_raw[2] * range_g) / 8192.0f) / 9.81f; // = accel * (1 / 32768.0f / 8.0f * 9.81f); + z_k[3] = raw.accelerometer_m_s2[0]; + z_k[4] = raw.accelerometer_raw[1]; + z_k[5] = raw.accelerometer_raw[2]; - // XXX Read out mag range via I2C on init, assuming 0.88 Ga and 12 bit res here - z_k[0] = (raw.magnetometer_raw[0] - mag_offset[0]) * 0.01f; - z_k[1] = (raw.magnetometer_raw[1] - mag_offset[1]) * 0.01f; - z_k[2] = (raw.magnetometer_raw[2] - mag_offset[2]) * 0.01f; + z_k[0] = raw.magnetometer_ga[0]; + z_k[1] = raw.magnetometer_ga[1]; + z_k[2] = raw.magnetometer_ga[2]; /* Fill in gyro measurements */ - z_k[6] = raw.gyro_raw[0] * 0.00026631611f /* = gyro * (500.0f / 180.0f * pi / 32768.0f ) */; - z_k[7] = raw.gyro_raw[1] * 0.00026631611f /* = gyro * (500.0f / 180.0f * pi / 32768.0f ) */; - z_k[8] = raw.gyro_raw[2] * 0.00026631611f /* = gyro * (500.0f / 180.0f * pi / 32768.0f ) */; - + z_k[6] = raw.gyro_rad_s[0]; + z_k[7] = raw.gyro_rad_s[1]; + z_k[8] = raw.gyro_rad_s[2]; uint64_t now = hrt_absolute_time(); unsigned int time_elapsed = now - last_run; @@ -212,19 +200,6 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) overloadcounter++; } -// now = hrt_absolute_time(); - /* filter values */ - /* - * function [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) - */ - - //extern void attitudeKalmanfilter(real32_T dt, const int8_T updVect[9], const real32_T z_k_data[9], - // const int32_T z_k_sizes[1], const real32_T u[4], - // const real32_T x_aposteriori_k[9], const real32_T P_aposteriori_k[81], - // const real32_T knownConst[20], real32_T eulerAngles[3], - // real32_T Rot_matrix[9], real32_T x_aposteriori[9], - // real32_T P_aposteriori[81]); - int8_t update_vect[9] = {1, 1, 1, 1, 1, 1, 1, 1, 1}; float euler[3]; int32_t z_k_sizes = 9; @@ -249,35 +224,20 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) printcounter++; -// time_elapsed = hrt_absolute_time() - now; -// if (blubb == 20) -// { -// printf("Estimator: %lu\n", time_elapsed); -// blubb = 0; -// } -// blubb++; - if (last_data > 0 && raw.timestamp - last_data > 8000) printf("sensor data missed! (%llu)\n", raw.timestamp - last_data); -// printf("%llu -> %llu = %llu\n", last_data, raw.timestamp, raw.timestamp - last_data); last_data = raw.timestamp; /* send out */ att.timestamp = raw.timestamp; - // att.roll = euler.x; - // att.pitch = euler.y; - // att.yaw = euler.z + F_M_PI; - - // if (att.yaw > 2 * F_M_PI) { - // att.yaw -= 2 * F_M_PI; - // } + att.roll = euler.x; + att.pitch = euler.y; + att.yaw = euler.z; // att.rollspeed = rates.x; // att.pitchspeed = rates.y; // att.yawspeed = rates.z; - // memcpy()R - // Broadcast orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); } From 16778b66e2a172b9c415e08288c2a5dbf5a0b7df Mon Sep 17 00:00:00 2001 From: patacongo Date: Thu, 20 Sep 2012 13:55:31 +0000 Subject: [PATCH 04/56] Oops.. the up_i2creset function mysteriously disappeared from stm32_i2c.c git-svn-id: http://svn.code.sf.net/p/nuttx/code/trunk@5167 42af7a65-404d-4744-a932-0658087f49c3 --- nuttx/arch/arm/src/stm32/Kconfig | 4 +- nuttx/arch/arm/src/stm32/stm32_i2c.c | 115 +++++++++++++++++++++++++++ 2 files changed, 117 insertions(+), 2 deletions(-) diff --git a/nuttx/arch/arm/src/stm32/Kconfig b/nuttx/arch/arm/src/stm32/Kconfig index 8b0cea32e4..a0f9a48921 100644 --- a/nuttx/arch/arm/src/stm32/Kconfig +++ b/nuttx/arch/arm/src/stm32/Kconfig @@ -1623,12 +1623,12 @@ config STM32_I2C_DYNTIMEO config STM32_I2C_DYNTIMEO_USECPERBYTE int "Timeout Microseconds per Byte" - default 0 + default 500 depends on STM32_I2C_DYNTIMEO config STM32_I2C_DYNTIMEO_STARTSTOP int "Timeout for Start/Stop (Milliseconds)" - default 5000 + default 1000 depends on STM32_I2C_DYNTIMEO config STM32_I2CTIMEOSEC diff --git a/nuttx/arch/arm/src/stm32/stm32_i2c.c b/nuttx/arch/arm/src/stm32/stm32_i2c.c index 2d1c365cd3..12a036f806 100644 --- a/nuttx/arch/arm/src/stm32/stm32_i2c.c +++ b/nuttx/arch/arm/src/stm32/stm32_i2c.c @@ -1967,4 +1967,119 @@ int up_i2cuninitialize(FAR struct i2c_dev_s * dev) return OK; } +/************************************************************************************ + * Name: up_i2creset + * + * Description: + * Reset an I2C bus + * + ************************************************************************************/ + +#ifdef CONFIG_I2C_RESET +int up_i2creset(FAR struct i2c_dev_s * dev) +{ + struct stm32_i2c_priv_s * priv; + unsigned int clock_count; + unsigned int stretch_count; + unit32_ scl_gpio; + unit32_ sda_gpio; + int ret = ERROR; + irqstate_t state; + + ASSERT(dev); + + /* Get I2C private structure */ + + priv = ((struct stm32_i2c_inst_s *)dev)->priv; + + /* Our caller must own a ref */ + + ASSERT(priv->refs > 0); + + /* Lock out other clients */ + + stm32_i2c_sem_wait(dev); + + /* De-init the port */ + + stm32_i2c_deinit(priv); + + /* Use GPIO configuration to un-wedge the bus */ + + scl_gpio = MKI2C_OUTPUT(priv->config->scl_pin); + sda_gpio = MKI2C_OUTPUT(priv->config->sda_pin); + + /* Clock the bus until any slaves currently driving it let it go. */ + + clock_count = 0; + while (!stm32_gpioread(sda_gpio)) + { + /* Give up if we have tried too hard */ + + if (clock_count++ > 1000) + { + goto out; + } + + /* Sniff to make sure that clock stretching has finished. + * + * If the bus never relaxes, the reset has failed. + */ + + stretch_count = 0; + while (!stm32_gpioread(scl_gpio)) + { + /* Give up if we have tried too hard */ + + if (stretch_count++ > 1000) + { + goto out; + } + + up_udelay(10); + } + + /* Drive SCL low */ + + stm32_gpiowrite(scl_gpio, 0); + up_udelay(10); + + /* Drive SCL high again */ + + stm32_gpiowrite(scl_gpio, 1); + up_udelay(10); + } + + /* Generate a start followed by a stop to reset slave + * state machines. + */ + + stm32_gpiowrite(sda_gpio, 0); + up_udelay(10); + stm32_gpiowrite(scl_gpio, 0); + up_udelay(10); + stm32_gpiowrite(scl_gpio, 1); + up_udelay(10); + stm32_gpiowrite(sda_gpio, 1); + up_udelay(10); + + /* Revert the GPIO configuration. */ + + stm32_unconfiggpio(sda_gpio); + stm32_unconfiggpio(scl_gpio); + + /* Re-init the port */ + + stm32_i2c_init(priv); + ret = OK; + +out: + + /* Release the port for re-use by other clients */ + + stm32_i2c_sem_post(dev); + return ret; +} +#endif /* CONFIG_I2C_RESET */ + #endif /* CONFIG_STM32_I2C1 || CONFIG_STM32_I2C2 || CONFIG_STM32_I2C3 */ From 082074f99196f8c936e21740a84b6738cb87875e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 21 Sep 2012 12:55:41 +0200 Subject: [PATCH 05/56] Completely implemented offboard control --- apps/attitude_estimator_ekf/Makefile | 2 +- .../attitude_estimator_ekf_main.c | 54 ++-- apps/commander/commander.c | 251 ++++++++++++------ apps/commander/state_machine_helper.c | 3 +- apps/mavlink/mavlink.c | 183 +++++++------ .../multirotor_att_control_main.c | 18 +- apps/sensors/sensors.cpp | 8 +- apps/uORB/objects_common.cpp | 4 +- apps/uORB/topics/manual_control_setpoint.h | 50 ++-- apps/uORB/topics/offboard_control_setpoint.h | 94 +++++++ apps/uORB/topics/vehicle_rates_setpoint.h | 1 - apps/uORB/topics/vehicle_status.h | 10 + 12 files changed, 467 insertions(+), 211 deletions(-) create mode 100644 apps/uORB/topics/offboard_control_setpoint.h diff --git a/apps/attitude_estimator_ekf/Makefile b/apps/attitude_estimator_ekf/Makefile index b4d62ed145..932b49f5c8 100644 --- a/apps/attitude_estimator_ekf/Makefile +++ b/apps/attitude_estimator_ekf/Makefile @@ -33,7 +33,7 @@ APPNAME = attitude_estimator_ekf PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 20000 +STACKSIZE = 2048 CSRCS = attitude_estimator_ekf_main.c \ codegen/eye.c \ diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index 2e294226a6..d7f448861c 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -98,7 +98,7 @@ static float P_aposteriori[81] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, }; /**< init: diagonal matrix with big values */ -static float knownConst[20] = {1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}; /**< knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */ +static float knownConst[15] = {1, 1, 1, 1, 1, 0.04, 4, 0.1, 70, 70, -2000, 9.81, 1, 4, 1}; /**< knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */ static float Rot_matrix[9] = {1.f, 0, 0, 0, 1.f, 0, 0, 0, 1.f @@ -149,7 +149,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) } thread_should_exit = false; - attitude_estimator_ekf_task = task_create("attitude_estimator_ekf", SCHED_PRIORITY_DEFAULT, 4096, attitude_estimator_ekf_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); + attitude_estimator_ekf_task = task_create("attitude_estimator_ekf", SCHED_PRIORITY_DEFAULT, 20000, attitude_estimator_ekf_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); } @@ -235,19 +235,19 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) dt = (raw.timestamp - last_measurement) / 1000000.0f; last_measurement = raw.timestamp; + /* Fill in gyro measurements */ + z_k[0] = raw.gyro_rad_s[0]; + z_k[1] = raw.gyro_rad_s[1]; + z_k[2] = raw.gyro_rad_s[2]; + /* scale from 14 bit to m/s2 */ z_k[3] = raw.accelerometer_m_s2[0]; z_k[4] = raw.accelerometer_raw[1]; z_k[5] = raw.accelerometer_raw[2]; - z_k[0] = raw.magnetometer_ga[0]; - z_k[1] = raw.magnetometer_ga[1]; - z_k[2] = raw.magnetometer_ga[2]; - - /* Fill in gyro measurements */ - z_k[6] = raw.gyro_rad_s[0]; - z_k[7] = raw.gyro_rad_s[1]; - z_k[8] = raw.gyro_rad_s[2]; + z_k[6] = raw.magnetometer_ga[0]; + z_k[7] = raw.magnetometer_ga[1]; + z_k[8] = raw.magnetometer_ga[2]; uint64_t now = hrt_absolute_time(); unsigned int time_elapsed = now - last_run; @@ -268,6 +268,24 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) int32_t z_k_sizes = 9; float u[4] = {0.0f, 0.0f, 0.0f, 0.0f}; + static bool const_initialized = false; + + /* initialize with good values once we have a reasonable dt estimate */ + if (!const_initialized && dt < 0.05 && dt > 0.005) + { + knownConst[0] = powf(0.6f, 2.0f*dt); + knownConst[1] = powf(0.6f, 2.0f*dt); + knownConst[2] = powf(0.2f, 2.0f*dt); + knownConst[3] = powf(0.2f, 2.0f*dt); + knownConst[4] = powf(0.000001f, 2.0f*dt); + const_initialized = true; + } + + /* do not execute the filter if not initialized */ + if (!const_initialized) { + continue; + } + uint64_t timing_start = hrt_absolute_time(); attitudeKalmanfilter(dt, update_vect, z_k, &z_k_sizes, u, x_aposteriori_k, P_aposteriori_k, knownConst, euler, Rot_matrix, x_aposteriori, P_aposteriori); @@ -277,15 +295,15 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) uint64_t timing_diff = hrt_absolute_time() - timing_start; /* print rotation matrix every 200th time */ - if (printcounter % 200 == 0) { - printf("EKF attitude iteration: %d, runtime: %d us, dt: %d us (%d Hz)\n", loopcounter, (int)timing_diff, (int)(dt * 1000000.0f), (int)(1.0f / dt)); - printf("roll: %8.4f\tpitch: %8.4f\tyaw:%8.4f", (double)euler[0], (double)euler[1], (double)euler[2]); - printf("\n%d\t%d\t%d\n%d\t%d\t%d\n%d\t%d\t%d\n", (int)(Rot_matrix[0] * 100), (int)(Rot_matrix[1] * 100), (int)(Rot_matrix[2] * 100), - (int)(Rot_matrix[3] * 100), (int)(Rot_matrix[4] * 100), (int)(Rot_matrix[5] * 100), - (int)(Rot_matrix[6] * 100), (int)(Rot_matrix[7] * 100), (int)(Rot_matrix[8] * 100)); - } + // if (printcounter % 200 == 0) { + // printf("EKF attitude iteration: %d, runtime: %d us, dt: %d us (%d Hz)\n", loopcounter, (int)timing_diff, (int)(dt * 1000000.0f), (int)(1.0f / dt)); + // printf("roll: %8.4f\tpitch: %8.4f\tyaw:%8.4f", (double)euler[0], (double)euler[1], (double)euler[2]); + // printf("\n%d\t%d\t%d\n%d\t%d\t%d\n%d\t%d\t%d\n", (int)(Rot_matrix[0] * 100), (int)(Rot_matrix[1] * 100), (int)(Rot_matrix[2] * 100), + // (int)(Rot_matrix[3] * 100), (int)(Rot_matrix[4] * 100), (int)(Rot_matrix[5] * 100), + // (int)(Rot_matrix[6] * 100), (int)(Rot_matrix[7] * 100), (int)(Rot_matrix[8] * 100)); + // } - printcounter++; + // printcounter++; if (last_data > 0 && raw.timestamp - last_data > 8000) printf("sensor data missed! (%llu)\n", raw.timestamp - last_data); diff --git a/apps/commander/commander.c b/apps/commander/commander.c index f5e143066a..011a7be97b 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -65,7 +65,8 @@ #include #include #include -#include +#include +#include #include #include #include @@ -83,7 +84,8 @@ #include - +PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0); /**< Go into low-level failsafe after 0 ms */ +//PARAM_DEFINE_INT32(SYS_FAILSAVE_HL, 0); /**< Go into high-level failsafe after 0 ms */ #include extern struct system_load_s system_load; @@ -94,14 +96,15 @@ extern struct system_load_s system_load; #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 STICK_ON_OFF_LIMIT 7500 -#define STICK_THRUST_RANGE 20000 +#define STICK_ON_OFF_LIMIT 0.75f +#define STICK_THRUST_RANGE 1.0f #define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 #define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) #define GPS_FIX_TYPE_2D 2 #define GPS_FIX_TYPE_3D 3 -#define GPS_QUALITY_GOOD_COUNTER_LIMIT 50 +#define GPS_QUALITY_GOOD_HYSTERIS_TIME_MS 5000 +#define GPS_QUALITY_GOOD_COUNTER_LIMIT (GPS_QUALITY_GOOD_HYSTERIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) /* File descriptors */ static int leds; @@ -114,6 +117,8 @@ static orb_advert_t stat_pub; static uint16_t nofix_counter = 0; static uint16_t gotfix_counter = 0; +static unsigned int failsafe_lowlevel_timeout_ms; + static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int deamon_task; /**< Handle of deamon task / thread */ @@ -1048,6 +1053,10 @@ int commander_thread_main(int argc, char *argv[]) /* not yet initialized */ commander_initialized = false; + /* set parameters */ + failsafe_lowlevel_timeout_ms = 0; + param_get(param_find("SYS_FAILSAVE_LL"), &failsafe_lowlevel_timeout_ms); + /* welcome user */ printf("[commander] I am in command now!\n"); @@ -1119,10 +1128,15 @@ int commander_thread_main(int argc, char *argv[]) int gps_quality_good_counter = 0; - /* Subscribe to RC data */ - int rc_sub = orb_subscribe(ORB_ID(rc_channels)); - struct rc_channels_s rc; - memset(&rc, 0, sizeof(rc)); + /* Subscribe to manual control data */ + int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + struct manual_control_setpoint_s sp_man; + memset(&sp_man, 0, sizeof(sp_man)); + + /* Subscribe to offboard control data */ + int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); + struct offboard_control_setpoint_s sp_offboard; + memset(&sp_offboard, 0, sizeof(sp_offboard)); int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); struct vehicle_gps_position_s gps; @@ -1141,11 +1155,15 @@ int commander_thread_main(int argc, char *argv[]) commander_initialized = true; uint64_t start_time = hrt_absolute_time(); + uint64_t failsave_ll_start_time = 0; + + bool state_changed = true; while (!thread_should_exit) { /* Get current values */ - orb_copy(ORB_ID(rc_channels), rc_sub, &rc); + orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man); + orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps); orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors); @@ -1256,10 +1274,10 @@ int commander_thread_main(int argc, char *argv[]) /* End battery voltage check */ /* Check if last transition deserved an audio event */ -#warning This code depends on state that is no longer? maintained -#if 0 - trigger_audio_alarm(vehicle_mode_previous, vehicle_state_previous, current_status.mode, current_status.state_machine); -#endif +// #warning This code depends on state that is no longer? maintained +// #if 0 +// trigger_audio_alarm(vehicle_mode_previous, vehicle_state_previous, current_status.mode, current_status.state_machine); +// #endif /* only check gps fix if we are outdoor */ // if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { @@ -1314,89 +1332,169 @@ int commander_thread_main(int argc, char *argv[]) /* end: check gps */ - /* Start RC state check */ - bool prev_lost = current_status.rc_signal_lost; + /* ignore RC signals if in offboard control mode */ + if (!current_status.offboard_control_signal_found_once) { + /* Start RC state check */ + bool prev_lost = current_status.rc_signal_lost; - if (rc.chan_count > 4 && (hrt_absolute_time() - rc.timestamp) < 100000) { + if ((hrt_absolute_time() - sp_man.timestamp) < 100000) { - /* quadrotor specific logic - check against system type in the future */ + /* check if left stick is in lower left position --> switch to standby state */ + if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && sp_man.throttle < STICK_THRUST_RANGE*0.2f) { //TODO: remove hardcoded values + if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { + update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd); + stick_on_counter = 0; - int16_t rc_yaw_scale = rc.chan[rc.function[YAW]].scale; - int16_t rc_throttle_scale = rc.chan[rc.function[THROTTLE]].scale; - int16_t mode_switch_rc_value = rc.chan[rc.function[OVERRIDE]].scale; - /* Check the value of the rc channel of the mode switch */ - mode_switch_rc_value = rc.chan[rc.function[OVERRIDE]].scale; + } else { + stick_off_counter++; + stick_on_counter = 0; + } + } - /* check if left stick is in lower left position --> switch to standby state */ - if (rc_yaw_scale < -STICK_ON_OFF_LIMIT && rc_throttle_scale < STICK_THRUST_RANGE*0.2f) { //TODO: remove hardcoded values - if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd); - stick_on_counter = 0; + /* check if left stick is in lower right position --> arm */ + if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE*0.2f) { //TODO: remove hardcoded values + if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { + update_state_machine_arm(stat_pub, ¤t_status, mavlink_fd); + stick_on_counter = 0; + + } else { + stick_on_counter++; + stick_off_counter = 0; + } + } + //printf("RC: y:%i/t:%i s:%i chans: %i\n", rc_yaw_scale, rc_throttle_scale, mode_switch_rc_value, rc.chan_count); + + if (sp_man.override_mode_switch > STICK_ON_OFF_LIMIT) { + update_state_machine_mode_manual(stat_pub, ¤t_status, mavlink_fd); + + } else if (sp_man.override_mode_switch < -STICK_ON_OFF_LIMIT) { + update_state_machine_mode_auto(stat_pub, ¤t_status, mavlink_fd); } else { - stick_off_counter++; - stick_on_counter = 0; + update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); } - } - - /* check if left stick is in lower right position --> arm */ - if (rc_yaw_scale > STICK_ON_OFF_LIMIT && rc_throttle_scale < STICK_THRUST_RANGE*0.2f) { //TODO: remove hardcoded values - if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - update_state_machine_arm(stat_pub, ¤t_status, mavlink_fd); - stick_on_counter = 0; + /* handle the case where RC signal was regained */ + if (!current_status.rc_signal_found_once) { + current_status.rc_signal_found_once = true; + mavlink_log_critical(mavlink_fd, "[commander] DETECTED RC SIGNAL FIRST TIME."); } else { - stick_on_counter++; - stick_off_counter = 0; + if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[commander] RECOVERY - RC SIGNAL GAINED!"); } - } - //printf("RC: y:%i/t:%i s:%i chans: %i\n", rc_yaw_scale, rc_throttle_scale, mode_switch_rc_value, rc.chan_count); - if (mode_switch_rc_value > STICK_ON_OFF_LIMIT) { - update_state_machine_mode_manual(stat_pub, ¤t_status, mavlink_fd); - - } else if (mode_switch_rc_value < -STICK_ON_OFF_LIMIT) { - update_state_machine_mode_auto(stat_pub, ¤t_status, mavlink_fd); + current_status.rc_signal_lost = false; + current_status.rc_signal_lost_interval = 0; } else { - update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); + static uint64_t last_print_time = 0; + /* print error message for first RC glitch and then every 5 s / 5000 ms) */ + if (!current_status.rc_signal_lost || ((hrt_absolute_time() - last_print_time) > 5000000)) { + /* only complain if the offboard control is NOT active */ + mavlink_log_critical(mavlink_fd, "[commander] CRITICAL - NO REMOTE SIGNAL!"); + last_print_time = hrt_absolute_time(); + } + /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ + current_status.rc_signal_cutting_off = true; + current_status.rc_signal_lost_interval = hrt_absolute_time() - sp_man.timestamp; + + /* if the RC signal is gone for a full second, consider it lost */ + if (current_status.rc_signal_lost_interval > 1000000) { + current_status.rc_signal_lost = true; + current_status.failsave_lowlevel = true; + } + + // if (hrt_absolute_time() - current_status.failsave_ll_start_time > failsafe_lowlevel_timeout_ms*1000) { + // publish_armed_status(¤t_status); + // } } - /* Publish RC signal */ - - - /* handle the case where RC signal was regained */ - if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[commander] RECOVERY - RC SIGNAL GAINED!"); - current_status.rc_signal_lost = false; - current_status.rc_signal_lost_interval = 0; - - } else { - static uint64_t last_print_time = 0; - /* print error message for first RC glitch and then every 5 s / 5000 ms) */ - if (!current_status.rc_signal_lost || ((hrt_absolute_time() - last_print_time) > 5000000)) { - mavlink_log_critical(mavlink_fd, "[commander] CRITICAL - NO REMOTE SIGNAL!"); - last_print_time = hrt_absolute_time(); + /* Check if this is the first loss or first gain*/ + if ((!prev_lost && current_status.rc_signal_lost) || + (prev_lost && !current_status.rc_signal_lost)) { + /* publish change */ + publish_armed_status(¤t_status); } - /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ - current_status.rc_signal_cutting_off = true; - current_status.rc_signal_lost_interval = hrt_absolute_time() - rc.timestamp; - - /* if the RC signal is gone for a full second, consider it lost */ - if (current_status.rc_signal_lost_interval > 1000000) current_status.rc_signal_lost = true; } - /* Check if this is the first loss or first gain*/ - if ((!prev_lost && current_status.rc_signal_lost) || - prev_lost && !current_status.rc_signal_lost) { - /* publish rc lost */ - publish_armed_status(¤t_status); - } + + /* End mode switch */ /* END RC state check */ + /* State machine update for offboard control */ + if (!current_status.rc_signal_found_once) { + if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) { + + /* set up control mode */ + if (current_status.flag_control_attitude_enabled != + (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE)) { + current_status.flag_control_attitude_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE); + state_changed = true; + } + + if (current_status.flag_control_rates_enabled != + (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES)) { + current_status.flag_control_attitude_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES); + state_changed = true; + } + + /* handle the case where RC signal was regained */ + if (!current_status.offboard_control_signal_found_once) { + current_status.offboard_control_signal_found_once = true; + /* enable offboard control, disable manual input */ + current_status.flag_control_manual_enabled = false; + current_status.flag_control_offboard_enabled = true; + state_changed = true; + + mavlink_log_critical(mavlink_fd, "[commander] DETECTED OFFBOARD CONTROL SIGNAL FIRST TIME."); + } else { + if (current_status.offboard_control_signal_lost) { + mavlink_log_critical(mavlink_fd, "[commander] RECOVERY - OFFBOARD CONTROL SIGNAL GAINED!"); + state_changed = true; + } + } + + current_status.offboard_control_signal_lost = false; + current_status.offboard_control_signal_lost_interval = 0; + + /* arm / disarm on request */ + if (sp_offboard.armed && !current_status.flag_system_armed) { + update_state_machine_arm(stat_pub, ¤t_status, mavlink_fd); + /* switch to stabilized mode = takeoff */ + update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); + } else if (!sp_offboard.armed && current_status.flag_system_armed) { + update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd); + } + + } else { + static uint64_t last_print_time = 0; + /* print error message for first RC glitch and then every 5 s / 5000 ms) */ + if (!current_status.offboard_control_signal_lost || ((hrt_absolute_time() - last_print_time) > 5000000)) { + /* only complain if the RC control is NOT active */ + mavlink_log_critical(mavlink_fd, "[commander] CRITICAL - NO OFFBOARD CONTROL SIGNAL!"); + last_print_time = hrt_absolute_time(); + } + /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ + current_status.offboard_control_signal_lost_interval = hrt_absolute_time() - sp_offboard.timestamp; + + /* if the signal is gone for 0.1 seconds, consider it lost */ + if (current_status.offboard_control_signal_lost_interval > 100000) { + current_status.offboard_control_signal_lost = true; + current_status.failsave_lowlevel_start_time = hrt_absolute_time(); + current_status.failsave_lowlevel = true; + + /* kill motors after timeout */ + if (hrt_absolute_time() - current_status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms*1000) { + state_changed = true; + } + } + } + } + + current_status.counter++; current_status.timestamp = hrt_absolute_time(); @@ -1411,8 +1509,10 @@ int commander_thread_main(int argc, char *argv[]) } /* publish at least with 1 Hz */ - if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || state_changed) { + publish_armed_status(¤t_status); orb_publish(ORB_ID(vehicle_status), stat_pub, ¤t_status); + state_changed = false; } /* Store old modes to detect and act on state transitions */ @@ -1430,7 +1530,8 @@ int commander_thread_main(int argc, char *argv[]) /* close fds */ led_deinit(); buzzer_deinit(); - close(rc_sub); + close(sp_man_sub); + close(sp_offboard_sub); close(gps_sub); close(sensor_sub); diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index da85d0868e..e1d24d6a6d 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -223,7 +223,8 @@ void publish_armed_status(const struct vehicle_status_s *current_status) { /* lock down actuators if required */ // XXX FIXME Currently any loss of RC will completely disable all actuators // needs proper failsafe - armed.lockdown = (current_status->rc_signal_lost || current_status->flag_hil_enabled) ? true : false; + armed.lockdown = ((current_status->rc_signal_lost && current_status->offboard_control_signal_lost) + || current_status->flag_hil_enabled) ? true : false; orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); } diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 6470e65c89..7eca3031d9 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -65,7 +65,7 @@ #include #include #include -#include +#include #include #include #include @@ -126,7 +126,7 @@ static struct vehicle_attitude_s hil_attitude; static struct vehicle_global_position_s hil_global_pos; -static struct vehicle_rates_setpoint_s vehicle_rates_sp; +static struct offboard_control_setpoint_s offboard_control_sp; static struct vehicle_command_s vcmd; @@ -188,9 +188,9 @@ static struct mavlink_subscriptions { }; static struct mavlink_publications { - orb_advert_t vehicle_rates_sp_pub; + orb_advert_t offboard_control_sp_pub; } mavlink_pubs = { - .vehicle_rates_sp_pub = -1 + .offboard_control_sp_pub = -1 }; @@ -1138,11 +1138,6 @@ mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) } } -#define MAVLINK_OFFBOARD_CONTROL_MODE_NONE 0 -#define MAVLINK_OFFBOARD_CONTROL_MODE_RATES 1 -#define MAVLINK_OFFBOARD_CONTROL_MODE_ATTITUDE 2 -#define MAVLINK_OFFBOARD_CONTROL_MODE_VELOCITY 3 -#define MAVLINK_OFFBOARD_CONTROL_MODE_POSITION 4 #define MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED 0x10 /**************************************************************************** @@ -1254,81 +1249,105 @@ void handleMessage(mavlink_message_t *msg) // printf("got MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT target_system=%u, sysid = %u\n", quad_motors_setpoint.target_system, mavlink_system.sysid); if (mavlink_system.sysid < 4) { - /* only send if RC is off */ - if (v_status.rc_signal_lost) { + /* + * rate control mode - defined by MAVLink + */ - /* rate control mode */ - if (quad_motors_setpoint.mode == 1) { - vehicle_rates_sp.roll = quad_motors_setpoint.roll[mavlink_system.sysid] / (float)INT16_MAX; - vehicle_rates_sp.pitch = quad_motors_setpoint.pitch[mavlink_system.sysid] / (float)INT16_MAX; - vehicle_rates_sp.yaw = quad_motors_setpoint.yaw[mavlink_system.sysid] / (float)INT16_MAX; - vehicle_rates_sp.thrust = quad_motors_setpoint.thrust[mavlink_system.sysid] / (float)UINT16_MAX; + uint8_t ml_mode = 0; + bool ml_armed = false; - vehicle_rates_sp.timestamp = hrt_absolute_time(); - } - - /* check if topic has to be advertised */ - if (mavlink_pubs.vehicle_rates_sp_pub <= 0) { - mavlink_pubs.vehicle_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &vehicle_rates_sp); - } - /* Publish */ - orb_publish(ORB_ID(vehicle_rates_setpoint), mavlink_pubs.vehicle_rates_sp_pub, &vehicle_rates_sp); - - /* change armed status if required */ - bool cmd_armed = (quad_motors_setpoint.mode & MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED); - - bool cmd_generated = false; - - if (v_status.flag_control_offboard_enabled != cmd_armed) { - vcmd.param1 = cmd_armed; - vcmd.param2 = 0; - vcmd.param3 = 0; - vcmd.param4 = 0; - vcmd.param5 = 0; - vcmd.param6 = 0; - vcmd.param7 = 0; - vcmd.command = MAV_CMD_COMPONENT_ARM_DISARM; - vcmd.target_system = mavlink_system.sysid; - vcmd.target_component = MAV_COMP_ID_ALL; - vcmd.source_system = msg->sysid; - vcmd.source_component = msg->compid; - vcmd.confirmation = 1; - - cmd_generated = true; - } - - /* check if input has to be enabled */ - if ((v_status.flag_control_rates_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_RATES)) || - (v_status.flag_control_attitude_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_ATTITUDE)) || - (v_status.flag_control_velocity_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_VELOCITY)) || - (v_status.flag_control_position_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_POSITION))) { - vcmd.param1 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_RATES); - vcmd.param2 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_ATTITUDE); - vcmd.param3 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_VELOCITY); - vcmd.param4 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_POSITION); - vcmd.param5 = 0; - vcmd.param6 = 0; - vcmd.param7 = 0; - vcmd.command = PX4_CMD_CONTROLLER_SELECTION; - vcmd.target_system = mavlink_system.sysid; - vcmd.target_component = MAV_COMP_ID_ALL; - vcmd.source_system = msg->sysid; - vcmd.source_component = msg->compid; - vcmd.confirmation = 1; - - cmd_generated = true; - } - - if (cmd_generated) { - /* check if topic is advertised */ - if (cmd_pub <= 0) { - cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); - } else { - /* create command */ - orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); - } - } + if (quad_motors_setpoint.mode & MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED) { + ml_armed = true; } + + switch (quad_motors_setpoint.mode) { + case 0: + break; + case 1: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES; + break; + case 2: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; + break; + case 3: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY; + break; + case 4: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION; + break; + } + + offboard_control_sp.p1 = quad_motors_setpoint.roll[mavlink_system.sysid] / (float)INT16_MAX; + offboard_control_sp.p2 = quad_motors_setpoint.pitch[mavlink_system.sysid] / (float)INT16_MAX; + offboard_control_sp.p3= quad_motors_setpoint.yaw[mavlink_system.sysid] / (float)INT16_MAX; + offboard_control_sp.p4 = quad_motors_setpoint.thrust[mavlink_system.sysid] / (float)UINT16_MAX; + offboard_control_sp.armed = ml_armed; + offboard_control_sp.mode = ml_mode; + + offboard_control_sp.timestamp = hrt_absolute_time(); + + /* check if topic has to be advertised */ + if (mavlink_pubs.offboard_control_sp_pub <= 0) { + mavlink_pubs.offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); + } else { + /* Publish */ + orb_publish(ORB_ID(offboard_control_setpoint), mavlink_pubs.offboard_control_sp_pub, &offboard_control_sp); + } + + // /* change armed status if required */ + // bool cmd_armed = (quad_motors_setpoint.mode & MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED); + + // bool cmd_generated = false; + + // if (v_status.flag_control_offboard_enabled != cmd_armed) { + // vcmd.param1 = cmd_armed; + // vcmd.param2 = 0; + // vcmd.param3 = 0; + // vcmd.param4 = 0; + // vcmd.param5 = 0; + // vcmd.param6 = 0; + // vcmd.param7 = 0; + // vcmd.command = MAV_CMD_COMPONENT_ARM_DISARM; + // vcmd.target_system = mavlink_system.sysid; + // vcmd.target_component = MAV_COMP_ID_ALL; + // vcmd.source_system = msg->sysid; + // vcmd.source_component = msg->compid; + // vcmd.confirmation = 1; + + // cmd_generated = true; + // } + + // /* check if input has to be enabled */ + // if ((v_status.flag_control_rates_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_RATES)) || + // (v_status.flag_control_attitude_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_ATTITUDE)) || + // (v_status.flag_control_velocity_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_VELOCITY)) || + // (v_status.flag_control_position_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_POSITION))) { + // vcmd.param1 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_RATES); + // vcmd.param2 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_ATTITUDE); + // vcmd.param3 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_VELOCITY); + // vcmd.param4 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_POSITION); + // vcmd.param5 = 0; + // vcmd.param6 = 0; + // vcmd.param7 = 0; + // vcmd.command = PX4_CMD_CONTROLLER_SELECTION; + // vcmd.target_system = mavlink_system.sysid; + // vcmd.target_component = MAV_COMP_ID_ALL; + // vcmd.source_system = msg->sysid; + // vcmd.source_component = msg->compid; + // vcmd.confirmation = 1; + + // cmd_generated = true; + // } + + // if (cmd_generated) { + // /* check if topic is advertised */ + // if (cmd_pub <= 0) { + // cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); + // } else { + // /* create command */ + // orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); + // } + // } } } @@ -1542,7 +1561,7 @@ int mavlink_thread_main(int argc, char *argv[]) memset(&rc, 0, sizeof(rc)); memset(&hil_attitude, 0, sizeof(hil_attitude)); memset(&hil_global_pos, 0, sizeof(hil_global_pos)); - memset(&vehicle_rates_sp, 0, sizeof(vehicle_rates_sp)); + memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); memset(&vcmd, 0, sizeof(vcmd)); /* print welcome text */ diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 3f0d562459..6cf811158d 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -58,6 +58,7 @@ #include #include #include +#include #include #include #include @@ -94,6 +95,8 @@ mc_thread_main(int argc, char *argv[]) memset(&manual, 0, sizeof(manual)); struct sensor_combined_s raw; memset(&raw, 0, sizeof(raw)); + struct offboard_control_setpoint_s offboard_sp; + memset(&offboard_sp, 0, sizeof(offboard_sp)); struct vehicle_rates_setpoint_s rates_sp; memset(&rates_sp, 0, sizeof(rates_sp)); @@ -102,7 +105,7 @@ mc_thread_main(int argc, char *argv[]) /* subscribe to attitude, motor setpoints and system state */ int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - int setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); + int setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); int state_sub = orb_subscribe(ORB_ID(vehicle_status)); int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); @@ -150,7 +153,7 @@ mc_thread_main(int argc, char *argv[]) bool updated; orb_check(setpoint_sub, &updated); if (updated) { - orb_copy(ORB_ID(vehicle_rates_setpoint), setpoint_sub, &rates_sp); + orb_copy(ORB_ID(offboard_control_setpoint), setpoint_sub, &offboard_sp); } /* get a local copy of the current sensor values */ orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); @@ -177,6 +180,17 @@ mc_thread_main(int argc, char *argv[]) } } else if (state.flag_control_offboard_enabled) { /* offboard inputs */ + if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) { + rates_sp.roll = offboard_sp.p1; + rates_sp.pitch = offboard_sp.p2; + rates_sp.yaw = offboard_sp.p3; + rates_sp.thrust = offboard_sp.p4; + } else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) { + att_sp.roll_body = offboard_sp.p1; + att_sp.pitch_body = offboard_sp.p2; + att_sp.yaw_body = offboard_sp.p3; + att_sp.thrust = offboard_sp.p4; + } /* decide wether we want rate or position input */ } diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index bd1b7b37da..eb22ac8a70 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -567,7 +567,7 @@ Sensors::accel_init() _fd_bma180 = open("/dev/bma180", O_RDONLY); if (_fd_bma180 < 0) { warn("/dev/bma180"); - errx(1, "FATAL: no accelerometer found"); + warn("FATAL: no accelerometer found"); } /* discard first (junk) reading */ @@ -600,7 +600,7 @@ Sensors::gyro_init() _fd_gyro_l3gd20 = open("/dev/l3gd20", O_RDONLY); if (_fd_gyro_l3gd20 < 0) { warn("/dev/l3gd20"); - errx(1, "FATAL: no gyro found"); + warn("FATAL: no gyro found"); } /* discard first (junk) reading */ @@ -988,6 +988,8 @@ Sensors::ppm_poll() _rc.chan_count = ppm_decoded_channels; _rc.timestamp = ppm_last_valid_decode; + manual_control.timestamp = ppm_last_valid_decode; + /* roll input - rolling right is stick-wise and rotation-wise positive */ manual_control.roll = _rc.chan[_rc.function[ROLL]].scaled; if (manual_control.roll < -1.0f) manual_control.roll = -1.0f; @@ -1090,7 +1092,7 @@ Sensors::task_main() /* advertise the manual_control topic */ { struct manual_control_setpoint_s manual_control; - manual_control.mode = ROLLPOS_PITCHPOS_YAWRATE_THROTTLE; + manual_control.mode = MANUAL_CONTROL_MODE_ATT_YAW_RATE; manual_control.roll = 0.0f; manual_control.pitch = 0.0f; manual_control.yaw = 0.0f; diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp index be85a345ae..f211648a90 100644 --- a/apps/uORB/objects_common.cpp +++ b/apps/uORB/objects_common.cpp @@ -59,7 +59,6 @@ ORB_DEFINE(output_pwm, struct pwm_output_values); #include ORB_DEFINE(input_rc, struct rc_input_values); -// XXX need to check wether these should be here #include "topics/vehicle_attitude.h" ORB_DEFINE(vehicle_attitude, struct vehicle_attitude_s); @@ -99,6 +98,9 @@ ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s); #include "topics/manual_control_setpoint.h" ORB_DEFINE(manual_control_setpoint, struct manual_control_setpoint_s); +#include "topics/offboard_control_setpoint.h" +ORB_DEFINE(offboard_control_setpoint, struct offboard_control_setpoint_s); + #include "topics/optical_flow.h" ORB_DEFINE(optical_flow, struct optical_flow_s); diff --git a/apps/uORB/topics/manual_control_setpoint.h b/apps/uORB/topics/manual_control_setpoint.h index b01c7438d9..a7f5be708d 100644 --- a/apps/uORB/topics/manual_control_setpoint.h +++ b/apps/uORB/topics/manual_control_setpoint.h @@ -1,21 +1,21 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: @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. + * 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. + * 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. + * 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 @@ -57,31 +57,27 @@ */ enum MANUAL_CONTROL_MODE { - DIRECT = 0, - ROLLPOS_PITCHPOS_YAWRATE_THROTTLE = 1, - ROLLRATE_PITCHRATE_YAWRATE_THROTTLE = 2, - ROLLPOS_PITCHPOS_YAWPOS_THROTTLE = 3 + MANUAL_CONTROL_MODE_DIRECT = 0, + MANUAL_CONTROL_MODE_ATT_YAW_RATE = 1, + MANUAL_CONTROL_MODE_ATT_YAW_POS = 2, + MANUAL_CONTROL_MODE_MULTIROTOR_SIMPLE = 3 /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */ }; struct manual_control_setpoint_s { + uint64_t timestamp; - enum MANUAL_CONTROL_MODE mode; /**< The current control inputs mode */ - float roll; /**< roll / roll rate input */ - float pitch; - float yaw; - float throttle; + enum MANUAL_CONTROL_MODE mode; /**< The current control inputs mode */ + float roll; /**< ailerons roll / roll rate input */ + float pitch; /**< elevator / pitch / pitch rate */ + float yaw; /**< rudder / yaw rate / yaw */ + float throttle; /**< throttle / collective thrust / altitude */ - float override_mode_switch; + float override_mode_switch; - float ailerons; - float elevator; - float rudder; - float flaps; - - float aux1_cam_pan; - float aux2_cam_tilt; - float aux3_cam_zoom; - float aux4_cam_roll; + float aux1_cam_pan_flaps; + float aux2_cam_tilt; + float aux3_cam_zoom; + float aux4_cam_roll; }; /**< manual control inputs */ diff --git a/apps/uORB/topics/offboard_control_setpoint.h b/apps/uORB/topics/offboard_control_setpoint.h new file mode 100644 index 0000000000..2e895c59c4 --- /dev/null +++ b/apps/uORB/topics/offboard_control_setpoint.h @@ -0,0 +1,94 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: @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 offboard_control_setpoint.h + * Definition of the manual_control_setpoint uORB topic. + */ + +#ifndef TOPIC_OFFBOARD_CONTROL_SETPOINT_H_ +#define TOPIC_OFFBOARD_CONTROL_SETPOINT_H_ + +#include +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +/** + * Off-board control inputs. + * + * Typically sent by a ground control station / joystick or by + * some off-board controller via C or SIMULINK. + */ +enum OFFBOARD_CONTROL_MODE +{ + OFFBOARD_CONTROL_MODE_DIRECT = 0, + OFFBOARD_CONTROL_MODE_DIRECT_RATES = 1, + OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE = 2, + OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY = 3, + OFFBOARD_CONTROL_MODE_DIRECT_POSITION = 4, + OFFBOARD_CONTROL_MODE_ATT_YAW_RATE = 5, + OFFBOARD_CONTROL_MODE_ATT_YAW_POS = 6, + OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 7, /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */ +}; + +struct offboard_control_setpoint_s { + uint64_t timestamp; + + enum OFFBOARD_CONTROL_MODE mode; /**< The current control inputs mode */ + bool armed; /**< Armed flag set, yes / no */ + float p1; /**< ailerons roll / roll rate input */ + float p2; /**< elevator / pitch / pitch rate */ + float p3; /**< rudder / yaw rate / yaw */ + float p4; /**< throttle / collective thrust / altitude */ + + float override_mode_switch; + + float aux1_cam_pan_flaps; + float aux2_cam_tilt; + float aux3_cam_zoom; + float aux4_cam_roll; + +}; /**< offboard control inputs */ +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(offboard_control_setpoint); + +#endif diff --git a/apps/uORB/topics/vehicle_rates_setpoint.h b/apps/uORB/topics/vehicle_rates_setpoint.h index 3400e53999..46e62c4b71 100644 --- a/apps/uORB/topics/vehicle_rates_setpoint.h +++ b/apps/uORB/topics/vehicle_rates_setpoint.h @@ -47,7 +47,6 @@ * @addtogroup topics * @{ */ - struct vehicle_rates_setpoint_s { uint64_t timestamp; /**< in microseconds since system start */ diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index 850029f015..c727527b16 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -110,6 +110,8 @@ struct vehicle_status_s uint16_t counter; /**< incremented by the writing thread everytime new data is stored */ uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ + uint64_t failsave_lowlevel_start_time; /**< time when the lowlevel failsafe flag was set */ + //uint64_t failsave_highlevel_begin; TO BE COMPLETED commander_state_machine_t state_machine; /**< current flight state, main state machine */ enum VEHICLE_FLIGHT_MODE flight_mode; /**< current flight mode, as defined by mode switch */ @@ -134,10 +136,18 @@ struct vehicle_status_s bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */ bool flag_preflight_accel_calibration; + bool rc_signal_found_once; bool rc_signal_lost; /**< true if RC reception is terminally lost */ bool rc_signal_cutting_off; /**< true if RC reception is weak / cutting off */ uint64_t rc_signal_lost_interval; /**< interval in microseconds since when no RC signal is available */ + bool offboard_control_signal_found_once; + bool offboard_control_signal_lost; + uint64_t offboard_control_signal_lost_interval; /**< interval in microseconds without an offboard control message */ + + bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */ + //bool failsave_highlevel; + /* see SYS_STATUS mavlink message for the following */ uint32_t onboard_control_sensors_present; uint32_t onboard_control_sensors_enabled; From 8a11f76994f74e4b38e861d559b305c707d78190 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 21 Sep 2012 14:42:57 +0200 Subject: [PATCH 06/56] Updated C files for attitude estimator --- .../attitude_estimator_ekf_main.c | 17 ++++++++++------- .../codegen/attitudeKalmanfilter.c | 2 +- .../codegen/attitudeKalmanfilter.h | 2 +- .../codegen/attitudeKalmanfilter_initialize.c | 2 +- .../codegen/attitudeKalmanfilter_initialize.h | 2 +- .../codegen/attitudeKalmanfilter_terminate.c | 2 +- .../codegen/attitudeKalmanfilter_terminate.h | 2 +- .../codegen/attitudeKalmanfilter_types.h | 2 +- apps/attitude_estimator_ekf/codegen/cross.c | 2 +- apps/attitude_estimator_ekf/codegen/cross.h | 2 +- apps/attitude_estimator_ekf/codegen/diag.c | 2 +- apps/attitude_estimator_ekf/codegen/diag.h | 2 +- apps/attitude_estimator_ekf/codegen/eye.c | 2 +- apps/attitude_estimator_ekf/codegen/eye.h | 2 +- apps/attitude_estimator_ekf/codegen/find.c | 2 +- apps/attitude_estimator_ekf/codegen/find.h | 2 +- apps/attitude_estimator_ekf/codegen/mrdivide.c | 2 +- apps/attitude_estimator_ekf/codegen/mrdivide.h | 2 +- apps/attitude_estimator_ekf/codegen/norm.c | 2 +- apps/attitude_estimator_ekf/codegen/norm.h | 2 +- apps/attitude_estimator_ekf/codegen/rtGetInf.c | 2 +- apps/attitude_estimator_ekf/codegen/rtGetInf.h | 2 +- apps/attitude_estimator_ekf/codegen/rtGetNaN.c | 2 +- apps/attitude_estimator_ekf/codegen/rtGetNaN.h | 2 +- .../attitude_estimator_ekf/codegen/rt_defines.h | 2 +- .../codegen/rt_nonfinite.c | 2 +- .../codegen/rt_nonfinite.h | 2 +- apps/attitude_estimator_ekf/codegen/rtwtypes.h | 2 +- apps/commander/commander.c | 6 ++++++ apps/mavlink/mavlink.c | 12 ++++++------ apps/sdlog/sdlog.c | 4 +++- 31 files changed, 52 insertions(+), 41 deletions(-) diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index d7f448861c..d04556e125 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -70,7 +70,7 @@ __EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]); // #define PROJECTION_INITIALIZE_COUNTER_LIMIT 5000 // #define REPROJECTION_COUNTER_LIMIT 125 -static unsigned int loop_interval_alarm = 4500; // loop interval in microseconds +static unsigned int loop_interval_alarm = 6500; // loop interval in microseconds static float dt = 1; /* 0, 0, -9.81, 1, 1, 1, wo (gyro offset), w */ @@ -205,6 +205,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) /* subscribe to raw data */ int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); + /* rate-limit raw data updates to 200Hz */ + orb_set_interval(sub_raw, 5); + /* advertise attitude */ orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); @@ -222,11 +225,11 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) }; int ret = poll(fds, 1, 1000); - /* check for a timeout */ - if (ret == 0) { - /* */ - - /* update successful, copy data on every 2nd run and execute filter */ + if (ret < 0) { + /* XXX this is seriously bad - should be an emergency */ + } else if (ret == 0) { + /* XXX this means no sensor data - should be critical or emergency */ + printf("[attitude estimator ekf] WARNING: Not getting sensor data - sensor app running?\n"); } else { orb_copy(ORB_ID(sensor_combined), sub_raw, &raw); @@ -256,7 +259,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) if (time_elapsed > loop_interval_alarm) { //TODO: add warning, cpu overload here if (overloadcounter == 20) { - printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR BLACK MAGIC (%lu > %lu)\n", time_elapsed, loop_interval_alarm); + printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm); overloadcounter = 0; } diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c index 321e6874d0..bfbb3be012 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Mon Sep 17 20:13:22 2012 + * C source code generated on: Fri Sep 21 13:56:42 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h index 8207aa5c53..56f02b4c81 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Mon Sep 17 20:13:22 2012 + * C source code generated on: Fri Sep 21 13:56:43 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c index abf1519a67..b72256a09e 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter_initialize' * - * C source code generated on: Mon Sep 17 20:13:23 2012 + * C source code generated on: Fri Sep 21 13:56:44 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h index 20186f183c..efb465b255 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter_initialize' * - * C source code generated on: Mon Sep 17 20:13:24 2012 + * C source code generated on: Fri Sep 21 13:56:44 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c index 458ddb9eb0..d0bf625f09 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter_terminate' * - * C source code generated on: Mon Sep 17 20:13:24 2012 + * C source code generated on: Fri Sep 21 13:56:44 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h index a939eee200..1ad84575f8 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter_terminate' * - * C source code generated on: Mon Sep 17 20:13:24 2012 + * C source code generated on: Fri Sep 21 13:56:44 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h index 599a168848..bd83cc168a 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Mon Sep 17 20:13:22 2012 + * C source code generated on: Fri Sep 21 13:56:42 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/cross.c b/apps/attitude_estimator_ekf/codegen/cross.c index 49f655ece8..c888694536 100755 --- a/apps/attitude_estimator_ekf/codegen/cross.c +++ b/apps/attitude_estimator_ekf/codegen/cross.c @@ -3,7 +3,7 @@ * * Code generation for function 'cross' * - * C source code generated on: Mon Sep 17 20:13:23 2012 + * C source code generated on: Fri Sep 21 13:56:44 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/cross.h b/apps/attitude_estimator_ekf/codegen/cross.h index 844d8138fd..92e3a884d0 100755 --- a/apps/attitude_estimator_ekf/codegen/cross.h +++ b/apps/attitude_estimator_ekf/codegen/cross.h @@ -3,7 +3,7 @@ * * Code generation for function 'cross' * - * C source code generated on: Mon Sep 17 20:13:23 2012 + * C source code generated on: Fri Sep 21 13:56:44 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/diag.c b/apps/attitude_estimator_ekf/codegen/diag.c index e54d52a384..522f6e2851 100755 --- a/apps/attitude_estimator_ekf/codegen/diag.c +++ b/apps/attitude_estimator_ekf/codegen/diag.c @@ -3,7 +3,7 @@ * * Code generation for function 'diag' * - * C source code generated on: Mon Sep 17 20:13:22 2012 + * C source code generated on: Fri Sep 21 13:56:43 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/diag.h b/apps/attitude_estimator_ekf/codegen/diag.h index a4a2a4c829..bb92fb2424 100755 --- a/apps/attitude_estimator_ekf/codegen/diag.h +++ b/apps/attitude_estimator_ekf/codegen/diag.h @@ -3,7 +3,7 @@ * * Code generation for function 'diag' * - * C source code generated on: Mon Sep 17 20:13:23 2012 + * C source code generated on: Fri Sep 21 13:56:43 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/eye.c b/apps/attitude_estimator_ekf/codegen/eye.c index aece401c25..e67071dcea 100755 --- a/apps/attitude_estimator_ekf/codegen/eye.c +++ b/apps/attitude_estimator_ekf/codegen/eye.c @@ -3,7 +3,7 @@ * * Code generation for function 'eye' * - * C source code generated on: Mon Sep 17 20:13:23 2012 + * C source code generated on: Fri Sep 21 13:56:43 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/eye.h b/apps/attitude_estimator_ekf/codegen/eye.h index e715ae1c39..c07a1bc970 100755 --- a/apps/attitude_estimator_ekf/codegen/eye.h +++ b/apps/attitude_estimator_ekf/codegen/eye.h @@ -3,7 +3,7 @@ * * Code generation for function 'eye' * - * C source code generated on: Mon Sep 17 20:13:23 2012 + * C source code generated on: Fri Sep 21 13:56:43 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/find.c b/apps/attitude_estimator_ekf/codegen/find.c index 532ba43970..8f05ef146e 100755 --- a/apps/attitude_estimator_ekf/codegen/find.c +++ b/apps/attitude_estimator_ekf/codegen/find.c @@ -3,7 +3,7 @@ * * Code generation for function 'find' * - * C source code generated on: Mon Sep 17 20:13:22 2012 + * C source code generated on: Fri Sep 21 13:56:43 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/find.h b/apps/attitude_estimator_ekf/codegen/find.h index ceb90b6cf6..ca525c6009 100755 --- a/apps/attitude_estimator_ekf/codegen/find.h +++ b/apps/attitude_estimator_ekf/codegen/find.h @@ -3,7 +3,7 @@ * * Code generation for function 'find' * - * C source code generated on: Mon Sep 17 20:13:22 2012 + * C source code generated on: Fri Sep 21 13:56:43 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/mrdivide.c b/apps/attitude_estimator_ekf/codegen/mrdivide.c index 2fcaf8cb67..d098162d54 100755 --- a/apps/attitude_estimator_ekf/codegen/mrdivide.c +++ b/apps/attitude_estimator_ekf/codegen/mrdivide.c @@ -3,7 +3,7 @@ * * Code generation for function 'mrdivide' * - * C source code generated on: Mon Sep 17 20:13:23 2012 + * C source code generated on: Fri Sep 21 13:56:44 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/mrdivide.h b/apps/attitude_estimator_ekf/codegen/mrdivide.h index a58faaa262..e807afcc8b 100755 --- a/apps/attitude_estimator_ekf/codegen/mrdivide.h +++ b/apps/attitude_estimator_ekf/codegen/mrdivide.h @@ -3,7 +3,7 @@ * * Code generation for function 'mrdivide' * - * C source code generated on: Mon Sep 17 20:13:23 2012 + * C source code generated on: Fri Sep 21 13:56:44 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/norm.c b/apps/attitude_estimator_ekf/codegen/norm.c index 8d8e4e1109..97509a0c0c 100755 --- a/apps/attitude_estimator_ekf/codegen/norm.c +++ b/apps/attitude_estimator_ekf/codegen/norm.c @@ -3,7 +3,7 @@ * * Code generation for function 'norm' * - * C source code generated on: Mon Sep 17 20:13:23 2012 + * C source code generated on: Fri Sep 21 13:56:43 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/norm.h b/apps/attitude_estimator_ekf/codegen/norm.h index 479503ae36..63294717f4 100755 --- a/apps/attitude_estimator_ekf/codegen/norm.h +++ b/apps/attitude_estimator_ekf/codegen/norm.h @@ -3,7 +3,7 @@ * * Code generation for function 'norm' * - * C source code generated on: Mon Sep 17 20:13:23 2012 + * C source code generated on: Fri Sep 21 13:56:44 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rtGetInf.c b/apps/attitude_estimator_ekf/codegen/rtGetInf.c index 42b3af501f..f1bb71c873 100755 --- a/apps/attitude_estimator_ekf/codegen/rtGetInf.c +++ b/apps/attitude_estimator_ekf/codegen/rtGetInf.c @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Mon Sep 17 20:13:24 2012 + * C source code generated on: Fri Sep 21 13:56:45 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rtGetInf.h b/apps/attitude_estimator_ekf/codegen/rtGetInf.h index 20d8c7f058..6d32d51b56 100755 --- a/apps/attitude_estimator_ekf/codegen/rtGetInf.h +++ b/apps/attitude_estimator_ekf/codegen/rtGetInf.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Mon Sep 17 20:13:24 2012 + * C source code generated on: Fri Sep 21 13:56:45 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rtGetNaN.c b/apps/attitude_estimator_ekf/codegen/rtGetNaN.c index d29aea34b6..50581f34df 100755 --- a/apps/attitude_estimator_ekf/codegen/rtGetNaN.c +++ b/apps/attitude_estimator_ekf/codegen/rtGetNaN.c @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Mon Sep 17 20:13:24 2012 + * C source code generated on: Fri Sep 21 13:56:45 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rtGetNaN.h b/apps/attitude_estimator_ekf/codegen/rtGetNaN.h index a14b6170b8..12d8734f5b 100755 --- a/apps/attitude_estimator_ekf/codegen/rtGetNaN.h +++ b/apps/attitude_estimator_ekf/codegen/rtGetNaN.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Mon Sep 17 20:13:24 2012 + * C source code generated on: Fri Sep 21 13:56:44 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rt_defines.h b/apps/attitude_estimator_ekf/codegen/rt_defines.h index 4cad63ada3..419edf01c9 100755 --- a/apps/attitude_estimator_ekf/codegen/rt_defines.h +++ b/apps/attitude_estimator_ekf/codegen/rt_defines.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Mon Sep 17 20:13:24 2012 + * C source code generated on: Fri Sep 21 13:56:44 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c index bd04f52e66..42ff21d39e 100755 --- a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c +++ b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Mon Sep 17 20:13:24 2012 + * C source code generated on: Fri Sep 21 13:56:44 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h index 67b3ba205d..60128156ea 100755 --- a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h +++ b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Mon Sep 17 20:13:24 2012 + * C source code generated on: Fri Sep 21 13:56:44 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rtwtypes.h b/apps/attitude_estimator_ekf/codegen/rtwtypes.h index 6feb2e1a99..2709915e95 100755 --- a/apps/attitude_estimator_ekf/codegen/rtwtypes.h +++ b/apps/attitude_estimator_ekf/codegen/rtwtypes.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Mon Sep 17 20:13:24 2012 + * C source code generated on: Fri Sep 21 13:56:44 2012 * */ diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 011a7be97b..578bcd8752 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1365,12 +1365,18 @@ int commander_thread_main(int argc, char *argv[]) //printf("RC: y:%i/t:%i s:%i chans: %i\n", rc_yaw_scale, rc_throttle_scale, mode_switch_rc_value, rc.chan_count); if (sp_man.override_mode_switch > STICK_ON_OFF_LIMIT) { + current_status.flag_control_attitude_enabled = true; + current_status.flag_control_rates_enabled = false; update_state_machine_mode_manual(stat_pub, ¤t_status, mavlink_fd); } else if (sp_man.override_mode_switch < -STICK_ON_OFF_LIMIT) { + current_status.flag_control_attitude_enabled = true; + current_status.flag_control_rates_enabled = false; update_state_machine_mode_auto(stat_pub, ¤t_status, mavlink_fd); } else { + current_status.flag_control_attitude_enabled = true; + current_status.flag_control_rates_enabled = false; update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); } diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 7eca3031d9..f9c06099e9 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -1077,8 +1077,8 @@ static void *uorb_receiveloop(void *arg) if (fds[ifds++].revents & POLLIN) { /* copy local position data into local buffer */ orb_copy(ORB_ID(manual_control_setpoint), subs->man_control_sp_sub, &buf.man_control); - mavlink_msg_manual_control_send(MAVLINK_COMM_0, mavlink_system.sysid, buf.man_control.roll, - buf.man_control.pitch, buf.man_control.yaw, buf.man_control.throttle, 0); + mavlink_msg_manual_control_send(MAVLINK_COMM_0, mavlink_system.sysid, buf.man_control.roll*1000, + buf.man_control.pitch*1000, buf.man_control.yaw*1000, buf.man_control.throttle*1000, 0); } /* --- ACTUATOR ARMED --- */ @@ -1427,10 +1427,10 @@ void handleMessage(mavlink_message_t *msg) struct manual_control_setpoint_s mc; static orb_advert_t mc_pub = 0; - mc.roll = man.x; - mc.pitch = man.y; - mc.yaw = man.r; - mc.roll = man.z; + mc.roll = man.x*1000; + mc.pitch = man.y*1000; + mc.yaw = man.r*1000; + mc.roll = man.z*1000; /* fake RC channels with manual control input from simulator */ diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c index b99c6652c8..f1c1f9a239 100644 --- a/apps/sdlog/sdlog.c +++ b/apps/sdlog/sdlog.c @@ -289,6 +289,8 @@ int sdlog_thread_main(int argc, char *argv[]) { /* subscribe to ORB for sensors raw */ subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); fds[fdsc_count].fd = subs.sensor_sub; + /* rate-limit raw data updates to 200Hz */ + orb_set_interval(subs.sensor_sub, 5); fds[fdsc_count].events = POLLIN; fdsc_count++; @@ -459,7 +461,7 @@ int sdlog_thread_main(int argc, char *argv[]) { sysvector_bytes += write(sysvector_file, (const char*)&sysvector, sizeof(sysvector)); - usleep(4900); + usleep(10000); } unsigned bytes = sensor_combined_bytes + actuator_outputs_bytes + blackbox_file_bytes + actuator_controls_bytes; From 1d96f0b8536a7e3824f6010bfb2651a27ff03d71 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 21 Sep 2012 17:19:28 +0200 Subject: [PATCH 07/56] Fixed stupid interface bugs, working --- .../attitude_estimator_ekf_main.c | 15 ++++++++------- .../codegen/attitudeKalmanfilter.c | 8 ++++---- apps/attitude_estimator_ekf/codegen/norm.c | 4 ++-- apps/px4/ground_estimator/ground_estimator.c | 2 +- 4 files changed, 15 insertions(+), 14 deletions(-) diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index d04556e125..9ae4c50b78 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -245,8 +245,8 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) /* scale from 14 bit to m/s2 */ z_k[3] = raw.accelerometer_m_s2[0]; - z_k[4] = raw.accelerometer_raw[1]; - z_k[5] = raw.accelerometer_raw[2]; + z_k[4] = raw.accelerometer_m_s2[1]; + z_k[5] = raw.accelerometer_m_s2[2]; z_k[6] = raw.magnetometer_ga[0]; z_k[7] = raw.magnetometer_ga[1]; @@ -274,8 +274,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) static bool const_initialized = false; /* initialize with good values once we have a reasonable dt estimate */ - if (!const_initialized && dt < 0.05 && dt > 0.005) + if (!const_initialized /*&& dt < 0.05 && dt > 0.005*/) { + dt = 0.005f; knownConst[0] = powf(0.6f, 2.0f*dt); knownConst[1] = powf(0.6f, 2.0f*dt); knownConst[2] = powf(0.2f, 2.0f*dt); @@ -308,7 +309,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) // printcounter++; - if (last_data > 0 && raw.timestamp - last_data > 8000) printf("sensor data missed! (%llu)\n", raw.timestamp - last_data); + if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data); last_data = raw.timestamp; @@ -318,9 +319,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) att.pitch = euler[1]; att.yaw = euler[2]; - // att.rollspeed = rates.x; - // att.pitchspeed = rates.y; - // att.yawspeed = rates.z; + att.rollspeed = x_aposteriori[0]; + att.pitchspeed = x_aposteriori[1]; + att.yawspeed = x_aposteriori[2]; // Broadcast orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c index bfbb3be012..459dcc9b9d 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c @@ -35,7 +35,7 @@ static real32_T rt_atan2f_snf(real32_T u0, real32_T u1) if (rtIsNaNF(u0) || rtIsNaNF(u1)) { y = ((real32_T)rtNaN); } else if (rtIsInfF(u0) && rtIsInfF(u1)) { - y = (real32_T)atan2(u0 > 0.0F ? 1.0F : -1.0F, u1 > 0.0F ? 1.0F : -1.0F); + y = (real32_T)atan2f(u0 > 0.0F ? 1.0F : -1.0F, u1 > 0.0F ? 1.0F : -1.0F); } else if (u1 == 0.0F) { if (u0 > 0.0F) { y = RT_PIF / 2.0F; @@ -45,7 +45,7 @@ static real32_T rt_atan2f_snf(real32_T u0, real32_T u1) y = 0.0F; } } else { - y = (real32_T)atan2(u0, u1); + y = (real32_T)atan2f(u0, u1); } return y; @@ -484,7 +484,7 @@ void attitudeKalmanfilter(real32_T dt, const int8_T updVect[9], const real32_T x_n_b[ib] = z_k_data[ib + 3]; } - if ((real32_T)fabs(norm(x_n_b) - knownConst[11]) > knownConst[13]) { + if ((real32_T)fabsf(norm(x_n_b) - knownConst[11]) > knownConst[13]) { /* 'attitudeKalmanfilter:145' accUpt=10000; */ accUpt = 10000; } @@ -709,7 +709,7 @@ void attitudeKalmanfilter(real32_T dt, const int8_T updVect[9], const real32_T /* 'attitudeKalmanfilter:194' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */ /* 'attitudeKalmanfilter:195' eulerAngles=[phi;theta;psi]; */ eulerAngles[0] = rt_atan2f_snf(Rot_matrix[7], Rot_matrix[8]); - eulerAngles[1] = -(real32_T)asin(Rot_matrix[6]); + eulerAngles[1] = -(real32_T)asinf(Rot_matrix[6]); eulerAngles[2] = rt_atan2f_snf(Rot_matrix[3], Rot_matrix[0]); } diff --git a/apps/attitude_estimator_ekf/codegen/norm.c b/apps/attitude_estimator_ekf/codegen/norm.c index 97509a0c0c..fa07e1a90e 100755 --- a/apps/attitude_estimator_ekf/codegen/norm.c +++ b/apps/attitude_estimator_ekf/codegen/norm.c @@ -40,7 +40,7 @@ real32_T norm(const real32_T x[3]) firstNonZero = TRUE; for (k = 0; k < 3; k++) { if (x[k] != 0.0F) { - absxk = (real32_T)fabs(x[k]); + absxk = (real32_T)fabsf(x[k]); if (firstNonZero) { scale = absxk; y = 1.0F; @@ -56,7 +56,7 @@ real32_T norm(const real32_T x[3]) } } - return scale * (real32_T)sqrt(y); + return scale * (real32_T)sqrtf(y); } /* End of code generation (norm.c) */ diff --git a/apps/px4/ground_estimator/ground_estimator.c b/apps/px4/ground_estimator/ground_estimator.c index 07426ec2bd..ccf9ee3ec1 100644 --- a/apps/px4/ground_estimator/ground_estimator.c +++ b/apps/px4/ground_estimator/ground_estimator.c @@ -97,7 +97,7 @@ int ground_estimator_thread_main(int argc, char *argv[]) { /* XXX this is seriously bad - should be an emergency */ } else if (ret == 0) { /* XXX this means no sensor data - should be critical or emergency */ - printf("[attitude estimator bm] WARNING: Not getting sensor data - sensor app running?\n"); + printf("[ground estimator bm] WARNING: Not getting sensor data - sensor app running?\n"); } else { struct sensor_combined_s s; orb_copy(ORB_ID(sensor_combined), sub_raw, &s); From 6c7e21bd1cffdaa9ca33d2fc8ca5829a9fe2bd5e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 21 Sep 2012 19:14:50 +0200 Subject: [PATCH 08/56] Debugging output still enabled, fixed a number of additional issues --- .../attitude_estimator_ekf_main.c | 53 +++++++++++++++---- apps/mavlink/mavlink.c | 2 +- 2 files changed, 44 insertions(+), 11 deletions(-) diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index 9ae4c50b78..afc8fa77c1 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -55,6 +55,7 @@ #include #include #include +#include #include #include #include @@ -76,7 +77,7 @@ static float dt = 1; /* 0, 0, -9.81, 1, 1, 1, wo (gyro offset), w */ /* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */ static float z_k[9] = {0}; /**< Measurement vector */ -static float x_aposteriori_k[9] = {0}; /**< */ +static float x_aposteriori_k[9] = {0,0,0,0,0,-9.81,1,1,-1}; /**< */ static float x_aposteriori[9] = {0}; static float P_aposteriori_k[81] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, @@ -217,6 +218,10 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) thread_running = true; + /* advertise debug value */ + struct debug_key_value_s dbg = { .key = "", .value = 0.0f }; + orb_advert_t pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg); + /* Main loop*/ while (!thread_should_exit) { @@ -266,7 +271,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) overloadcounter++; } - int8_t update_vect[9] = {1, 1, 1, 1, 1, 1, 1, 1, 1}; + int8_t update_vect[9] = {1, 1, 1, 1, 1, 1, 0, 0, 0}; float euler[3]; int32_t z_k_sizes = 9; float u[4] = {0.0f, 0.0f, 0.0f, 0.0f}; @@ -277,11 +282,22 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) if (!const_initialized /*&& dt < 0.05 && dt > 0.005*/) { dt = 0.005f; - knownConst[0] = powf(0.6f, 2.0f*dt); - knownConst[1] = powf(0.6f, 2.0f*dt); - knownConst[2] = powf(0.2f, 2.0f*dt); - knownConst[3] = powf(0.2f, 2.0f*dt); - knownConst[4] = powf(0.000001f, 2.0f*dt); + knownConst[0] = 0.6f*0.6f*dt; + knownConst[1] = 0.6f*0.6f*dt; + knownConst[2] = 0.2f*0.2f*dt; + knownConst[3] = 0.2f*0.2f*dt; + knownConst[4] = 0.000001f*0.000001f*dt; // -9.81,1,1,-1}; + + x_aposteriori_k[0] = z_k[0]; + x_aposteriori_k[1] = z_k[1]; + x_aposteriori_k[2] = z_k[2]; + x_aposteriori_k[3] = z_k[3]; + x_aposteriori_k[4] = z_k[4]; + x_aposteriori_k[5] = z_k[5]; + x_aposteriori_k[6] = z_k[6]; + x_aposteriori_k[7] = z_k[7]; + x_aposteriori_k[8] = z_k[8]; + const_initialized = true; } @@ -299,15 +315,32 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) uint64_t timing_diff = hrt_absolute_time() - timing_start; /* print rotation matrix every 200th time */ - // if (printcounter % 200 == 0) { + if (printcounter % 2 == 0) { + // printf("x apo:\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n", + // x_aposteriori[0], x_aposteriori[1], x_aposteriori[2], + // x_aposteriori[3], x_aposteriori[4], x_aposteriori[5], + // x_aposteriori[6], x_aposteriori[7], x_aposteriori[8]); + + + // } + // printf("EKF attitude iteration: %d, runtime: %d us, dt: %d us (%d Hz)\n", loopcounter, (int)timing_diff, (int)(dt * 1000000.0f), (int)(1.0f / dt)); // printf("roll: %8.4f\tpitch: %8.4f\tyaw:%8.4f", (double)euler[0], (double)euler[1], (double)euler[2]); // printf("\n%d\t%d\t%d\n%d\t%d\t%d\n%d\t%d\t%d\n", (int)(Rot_matrix[0] * 100), (int)(Rot_matrix[1] * 100), (int)(Rot_matrix[2] * 100), // (int)(Rot_matrix[3] * 100), (int)(Rot_matrix[4] * 100), (int)(Rot_matrix[5] * 100), // (int)(Rot_matrix[6] * 100), (int)(Rot_matrix[7] * 100), (int)(Rot_matrix[8] * 100)); - // } + } - // printcounter++; + int i = printcounter % 9; + + // for (int i = 0; i < 9; i++) { + char name[10]; + sprintf(name, "xapo #%d", i); + memcpy(dbg.key, name, sizeof(dbg.key)); + dbg.value = x_aposteriori[i]; + orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg); + + printcounter++; if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data); diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index f9c06099e9..ce0a238dd0 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -1664,7 +1664,7 @@ int mavlink_thread_main(int argc, char *argv[]) set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 5); /* 200 Hz / 5 ms */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 5); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 5); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 3); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 5); /* 5 Hz */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 200); From a56b4ffe263a919befe1b77e382094e7bbba40c0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 21 Sep 2012 19:17:22 +0200 Subject: [PATCH 09/56] Enabled mag updates again --- apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index afc8fa77c1..867b484e1d 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -271,7 +271,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) overloadcounter++; } - int8_t update_vect[9] = {1, 1, 1, 1, 1, 1, 0, 0, 0}; + int8_t update_vect[9] = {1, 1, 1, 1, 1, 1, 1, 1, 1}; float euler[3]; int32_t z_k_sizes = 9; float u[4] = {0.0f, 0.0f, 0.0f, 0.0f}; From 4edf18b0098c8db0513c3aab76622bfa06fe5293 Mon Sep 17 00:00:00 2001 From: patacongo Date: Fri, 21 Sep 2012 17:32:30 +0000 Subject: [PATCH 10/56] Add support for Fire STM32v3; sscanf fixes from Kate git-svn-id: http://svn.code.sf.net/p/nuttx/code/trunk@5168 42af7a65-404d-4744-a932-0658087f49c3 --- apps/netutils/webserver/Kconfig | 4 +- nuttx/ChangeLog | 9 + nuttx/Documentation/NuttX.html | 6 +- nuttx/configs/Kconfig | 8 +- nuttx/configs/README.txt | 3 +- nuttx/configs/fire-stm32v2/Kconfig | 22 +++ nuttx/configs/fire-stm32v2/README.txt | 16 +- nuttx/configs/fire-stm32v2/nsh/defconfig | 17 +- .../configs/fire-stm32v2/src/fire-internal.h | 18 +- nuttx/lib/stdio/lib_sscanf.c | 164 +++++++++++++----- 10 files changed, 205 insertions(+), 62 deletions(-) diff --git a/apps/netutils/webserver/Kconfig b/apps/netutils/webserver/Kconfig index 78b0ff12ca..edca8dfdd2 100644 --- a/apps/netutils/webserver/Kconfig +++ b/apps/netutils/webserver/Kconfig @@ -6,6 +6,7 @@ config NETUTILS_WEBSERVER bool "uIP web server" default n + depends on NET_TCP ---help--- Enable support for the uIP web server. This tiny web server was from uIP 1.0, but has undergone many changes. It is, however, @@ -74,10 +75,11 @@ config NETUTILS_HTTPD_SERVERHEADER_DISABLE config NETUTILS_HTTPD_TIMEOUT int "Receive Timeout (sec)" default 0 + depends on NET_SOCKOPTS ---help--- Receive timeout setting (in seconds). A timeout value of zero disables the timeout. An HTTP 408 error is generated if the timeout - expires. + expires. This option depends on support for socket options (sockopts). choice prompt "File Transfer Method" diff --git a/nuttx/ChangeLog b/nuttx/ChangeLog index 7ad830d890..42110fd794 100644 --- a/nuttx/ChangeLog +++ b/nuttx/ChangeLog @@ -3377,3 +3377,12 @@ commands used in the build (Contributed by Richard Cochran). * drivers/net/enc28j60.c: The ENC28J60 Ethernet driver is now functional. + * configs/fire-stm32v2: Add support or the fire-stm32v3 board as + well (untested because I do not have a v3 board). + * lib/stdio/lib_sscanf.c: Add %n psuedo-format (from Kate). + * lib/stdio/lib_sscanf.c: There is an issue of handling input + when (1) no fieldwidth is provided and (2) there is no space + seperating the input values. No solutions is in place for this + case now (either space or a fieldwidth must be provided). But + at least some of the bad logic that attempted to handle this + case has been removed (noted by Kate). diff --git a/nuttx/Documentation/NuttX.html b/nuttx/Documentation/NuttX.html index 6b120f4e23..b51b5e7b32 100644 --- a/nuttx/Documentation/NuttX.html +++ b/nuttx/Documentation/NuttX.html @@ -1753,7 +1753,7 @@ STM32F103VCT chip. Contributed by Laurent Latil.
  • - This M3 Wildfire development board (STM32F103VET6), version 2. + The M3 Wildfire development board (STM32F103VET6), version 2. See http://firestm32.taobao.com (the current board is version 3).
    • @@ -1787,7 +1787,9 @@ Support for the Wildfire board was included in version 6.22 of NuttX. The board port is basically functional. Not all features have been verified. - The ENC28J60 network is not yet functional. + Support for FAT file system on an an SD card had been verified. + The ENC28J60 network is functional (but required lifting the chip select pin on the W25x16 part). + Customizations for the v3 version of the Wildfire board a selectable (but untested).

    diff --git a/nuttx/configs/Kconfig b/nuttx/configs/Kconfig index 12cc41ea72..752a5b0983 100644 --- a/nuttx/configs/Kconfig +++ b/nuttx/configs/Kconfig @@ -130,8 +130,8 @@ config ARCH_BOARD_EZ80F910200ZCO development kit, eZ80F091 part, and the Zilog ZDS-II Windows command line tools. The development environment is Cygwin under WinXP. -config ARCH_BOARD_FIRE_STM32V2 - bool "M3 Wildfire STM32v2 board" +config ARCH_BOARD_FIRE_STM32 + bool "M3 Wildfire STM3 board (v2 or v3)" depends on ARCH_CHIP_STM32F103VET6 select ARCH_HAVE_LEDS select ARCH_HAVE_BUTTONS @@ -607,7 +607,7 @@ config ARCH_BOARD default "ekk-lm3s9b96" if ARCH_BOARD_EKK_LM3S9B96 default "ez80f0910200kitg" if ARCH_BOARD_EZ80F910200KITG default "ez80f0910200zco" if ARCH_BOARD_EZ80F910200ZCO - default "fire-stm32v2" if ARCH_BOARD_FIRE_STM32V2 + default "fire-stm32v2" if ARCH_BOARD_FIRE_STM32 default "hymini-stm32v" if ARCH_BOARD_HYMINI_STM32V default "kwikstik-k40" if ARCH_BOARD_KWIKSTIK_K40 default "lincoln60" if ARCH_BOARD_LINCOLN60 @@ -747,7 +747,7 @@ endif if ARCH_BOARD_EZ80F910200ZCO source "configs/ez80f910200zco/Kconfig" endif -if ARCH_BOARD_FIRE_STM32V2 +if ARCH_BOARD_FIRE_STM32 source "configs/fire-stm32v2/Kconfig" endif if ARCH_BOARD_HYMINI_STM32V diff --git a/nuttx/configs/README.txt b/nuttx/configs/README.txt index b5ada6ee5d..5bbec874f7 100644 --- a/nuttx/configs/README.txt +++ b/nuttx/configs/README.txt @@ -1570,7 +1570,8 @@ configs/ez80f0910200zco configs/fire-stm32v2 A configuration for the M3 Wildfire STM32 board. This board is based on the - STM32F103VET6 chip. See http://firestm32.taobao.com + STM32F103VET6 chip. See http://firestm32.taobao.com . Version 2 and 3 of + the boards are supported but only version 2 has been tested. configs/hymini-stm32v A configuration for the HY-Mini STM32v board. This board is based on the diff --git a/nuttx/configs/fire-stm32v2/Kconfig b/nuttx/configs/fire-stm32v2/Kconfig index 3f4b857dad..4249a3dbde 100644 --- a/nuttx/configs/fire-stm32v2/Kconfig +++ b/nuttx/configs/fire-stm32v2/Kconfig @@ -4,3 +4,25 @@ # comment "M3 Wildfire Configuration" + +if ARCH_BOARD_FIRE_STM32 + +choice + prompt "Select Wildfire STM32 version" + default ARCH_BOARD_FIRE_STM32V2 + ---help--- + This port has logic differences to support either the Version 2 or + Version 3 of the Wildfire board. + +config ARCH_BOARD_FIRE_STM32V2 + bool "Wildfire STM32v2" + ---help--- + Selects the M3 Wildfire version 2. + +config ARCH_BOARD_FIRE_STM32V3 + bool "Wildfire STM32v3" + ---help--- + Selects the M3 Wildfire version 3. + +endchoice +endif diff --git a/nuttx/configs/fire-stm32v2/README.txt b/nuttx/configs/fire-stm32v2/README.txt index d9a4d14cad..6382a7363e 100644 --- a/nuttx/configs/fire-stm32v2/README.txt +++ b/nuttx/configs/fire-stm32v2/README.txt @@ -4,6 +4,10 @@ README This README discusses issues unique to NuttX configurations for the M3 Wildfire development board (STM32F103VET6). See http://firestm32.taobao.com +This configuration should support both the version 2 and version 3 of the +Wildfire board (using NuttX configuration options). However, only version 2 +has been verified. + Contents ======== @@ -27,8 +31,10 @@ PIN NAME SIGNAL NOTES 1 PE2 PE2-C-RCLK Camera (P9) 2 PE3 PE3-USB-M USB2.0 -3 PE4 PE4-BEEP LS1 Bell -4 PE5 (no name) 10Mbps ENC28J60 Interrupt +3 PE4 PE4-BEEP LS1 Bell (v2) + PE4 10Mbps ENC28J60 Interrupt (v3) +4 PE5 (no name) 10Mbps ENC28J60 Interrupt (v2) + PE5 KEY1, Low when closed (pulled high if open) (v3) 5 PE6 6 VBAT BT1 Battery (BT1) 7 PC13 Header 7X2 @@ -64,7 +70,8 @@ PIN NAME SIGNAL NOTES 32 PA7 PA7-SPI1-MOSI 2.4" TFT + Touchscreen, 10Mbit ENC28J60, SPI 2M FLASH 33 PC4 PC4-LED2 LED2, Active low (pulled high) 34 PC5 PC5-LED3 LED3, Active low (pulled high) -35 PB0 PB0-KEY1 KEY1, Low when closed (pulled high if open) +35 PB0 PB0-KEY1 KEY1, Low when closed (pulled high if open) (v2) + PB0 Header P5 (v3) 36 PB1 PB1-KEY2 KEY2, Low when closed (pulled high if open) 37 PB2 BOOT1/DGND 38 PE7 PE7-FSMC_D4 2.4" TFT + Touchscreen @@ -525,7 +532,8 @@ M3 Wildfire-specific Configuration Options CONFIG_ARCH_BOARD_name - For use in C code - CONFIG_ARCH_BOARD_FIRE_STM32V2=y + CONFIG_ARCH_BOARD_FIRE_STM32V2=y (Version 2) + CONFIG_ARCH_BOARD_FIRE_STM32V3=y (Version 3) CONFIG_ARCH_LOOPSPERMSEC - Must be calibrated for correct operation of delay loops diff --git a/nuttx/configs/fire-stm32v2/nsh/defconfig b/nuttx/configs/fire-stm32v2/nsh/defconfig index fd51878010..0802af3c94 100644 --- a/nuttx/configs/fire-stm32v2/nsh/defconfig +++ b/nuttx/configs/fire-stm32v2/nsh/defconfig @@ -150,6 +150,7 @@ CONFIG_STM32_USART2=y CONFIG_STM32_USB=y # CONFIG_STM32_WWDG is not set CONFIG_STM32_SPI=y +CONFIG_STM32_I2C=y # # Alternate Pin Mapping @@ -171,6 +172,15 @@ CONFIG_STM32_JTAG_FULL_ENABLE=y # CONFIG_STM32_SPI_INTERRUPTS is not set # CONFIG_STM32_SPI_DMA is not set +# +# I2C Configuration +# +# CONFIG_STM32_I2C_DYNTIMEO is not set +CONFIG_STM32_I2CTIMEOSEC=0 +CONFIG_STM32_I2CTIMEOMS=500 +CONFIG_STM32_I2CTIMEOTICKS=500 +# CONFIG_STM32_I2C_DUTY16_9 is not set + # # SDIO Configuration # @@ -215,7 +225,7 @@ CONFIG_BOOT_RUNFROMFLASH=y # # Board Selection # -CONFIG_ARCH_BOARD_FIRE_STM32V2=y +CONFIG_ARCH_BOARD_FIRE_STM32=y # CONFIG_ARCH_BOARD_CUSTOM is not set CONFIG_ARCH_BOARD="fire-stm32v2" @@ -238,6 +248,8 @@ CONFIG_NSH_MMCSDSPIPORTNO=0 # # M3 Wildfire Configuration # +CONFIG_ARCH_BOARD_FIRE_STM32V2=y +# CONFIG_ARCH_BOARD_FIRE_STM32V3 is not set # # RTOS Features @@ -316,6 +328,8 @@ CONFIG_I2C_TRANSFER=y # CONFIG_I2C_WRITEREAD is not set CONFIG_I2C_POLLED=y # CONFIG_I2C_TRACE is not set +CONFIG_ARCH_HAVE_I2CRESET=y +# CONFIG_I2C_RESET is not set CONFIG_SPI=y # CONFIG_SPI_OWNBUS is not set CONFIG_SPI_EXCHANGE=y @@ -348,6 +362,7 @@ CONFIG_ENC28J60_SPIMODE=0 CONFIG_ENC28J60_FREQUENCY=20000000 # CONFIG_ENC28J60_STATS is not set # CONFIG_ENC28J60_HALFDUPPLEX is not set +# CONFIG_ENC28J60_DUMPPACKET is not set # CONFIG_NET_E1000 is not set # CONFIG_NET_SLIP is not set # CONFIG_NET_VNET is not set diff --git a/nuttx/configs/fire-stm32v2/src/fire-internal.h b/nuttx/configs/fire-stm32v2/src/fire-internal.h index e9f7a85089..7ab78a08fe 100644 --- a/nuttx/configs/fire-stm32v2/src/fire-internal.h +++ b/nuttx/configs/fire-stm32v2/src/fire-internal.h @@ -166,7 +166,8 @@ * PIN NAME SIGNAL NOTES * --- ------ -------------- ------------------------------------------------------------------- * - * 35 PB0 PB0-KEY1 KEY1, Low when closed (pulled high if open) + * 35 PB0 PB0-KEY1 KEY1, Low when closed (pulled high if open) (v2) + * 35 PE5 PB0 KEY1, Low when closed (pulled high if open) (v3) * 36 PB1 PB1-KEY2 KEY2, Low when closed (pulled high if open) */ @@ -174,8 +175,13 @@ #define MAX_IRQBUTTON BUTTON_KEY2 #define NUM_IRQBUTTONS (MAX_IRQBUTTON - MIN_IRQBUTTON + 1) -#define GPIO_BTN_KEY1 (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|\ +#ifdef CONFIG_ARCH_BOARD_FIRE_STM32V3 +# define GPIO_BTN_KEY1 (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|\ + GPIO_EXTI|GPIO_PORTE|GPIO_PIN5) +#else +# define GPIO_BTN_KEY1 (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|\ GPIO_EXTI|GPIO_PORTB|GPIO_PIN0) +#endif #define GPIO_BTN_KEY2 (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|\ GPIO_EXTI|GPIO_PORTB|GPIO_PIN1) @@ -207,7 +213,8 @@ * 31 PA6 PA6-SPI1-MISO 2.4" TFT + Touchscreen, 10Mbit ENC28J60, SPI 2M FLASH * 32 PA7 PA7-SPI1-MOSI 2.4" TFT + Touchscreen, 10Mbit ENC28J60, SPI 2M FLASH * 98 PE1 PE1-FSMC_NBL1 2.4" TFT + Touchscreen, 10Mbit EN28J60 Reset - * 4 PE5 (no name) 10Mbps ENC28J60 Interrupt + * 4 PE5 (no name) 10Mbps ENC28J60 Interrupt (v2) + * 4 PE4 PE4 10Mbps ENC28J60 Interrupt (v3) */ #if defined(CONFIG_STM32_FSMC) && defined(CONFIG_ENC28J60) @@ -224,9 +231,14 @@ GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4) # define GPIO_ENC28J60_RESET (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN1) +#ifdef CONFIG_ARCH_BOARD_FIRE_STM32V3 +# define GPIO_ENC28J60_INTR (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|\ + GPIO_EXTI|GPIO_PORTE|GPIO_PIN4) +#else /* CONFIG_ARCH_BOARD_FIRE_STM32V2 */ # define GPIO_ENC28J60_INTR (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|\ GPIO_EXTI|GPIO_PORTE|GPIO_PIN5) #endif +#endif /* MP3 * diff --git a/nuttx/lib/stdio/lib_sscanf.c b/nuttx/lib/stdio/lib_sscanf.c index 01c96c21dd..11b5f387e8 100644 --- a/nuttx/lib/stdio/lib_sscanf.c +++ b/nuttx/lib/stdio/lib_sscanf.c @@ -1,7 +1,7 @@ /**************************************************************************** * lib/stdio/lib_sscanf.c * - * Copyright (C) 2007, 2008, 2011 Gregory Nutt. All rights reserved. + * Copyright (C) 2007, 2008, 2011-2012 Gregory Nutt. All rights reserved. * Author: Gregory Nutt * * Redistribution and use in source and binary forms, with or without @@ -63,7 +63,7 @@ * Global Function Prototypes ****************************************************************************/ -int vsscanf(char *buf, const char *s, va_list ap); +int vsscanf(char *buf, const char *fmt, va_list ap); /************************************************************************** * Global Constant Data @@ -109,67 +109,81 @@ int sscanf(FAR const char *buf, FAR const char *fmt, ...) * ANSI standard vsscanf implementation. * ****************************************************************************/ -int vsscanf(FAR char *buf, FAR const char *s, va_list ap) + +int vsscanf(FAR char *buf, FAR const char *fmt, va_list ap) { + FAR char *bufstart; + FAR char *tv; + FAR const char *tc; int count; int noassign; int width; int base = 10; int lflag; - FAR char *tv; - FAR const char *tc; char tmp[MAXLN]; - lvdbg("vsscanf: buf=\"%s\" fmt=\"%s\"\n", buf, s); + lvdbg("vsscanf: buf=\"%s\" fmt=\"%s\"\n", buf, fmt); - count = noassign = width = lflag = 0; - while (*s && *buf) + /* Remember the start of the input buffer. We will need this for %n + * calculations. + */ + + bufstart = buf; + + /* Parse the format, extracting values from the input buffer as needed */ + + count = 0; + noassign = 0; + width = 0; + lflag = 0; + + while (*fmt && *buf) { /* Skip over white space */ - while (isspace(*s)) + while (isspace(*fmt)) { - s++; + fmt++; } /* Check for a conversion specifier */ - if (*s == '%') + if (*fmt == '%') { lvdbg("vsscanf: Specifier found\n"); /* Check for qualifiers on the conversion specifier */ - s++; - for (; *s; s++) + fmt++; + for (; *fmt; fmt++) { - lvdbg("vsscanf: Processing %c\n", *s); + lvdbg("vsscanf: Processing %c\n", *fmt); - if (strchr("dibouxcsefg%", *s)) + if (strchr("dibouxcsefgn%", *fmt)) { break; } - if (*s == '*') + if (*fmt == '*') { noassign = 1; } - else if (*s == 'l' || *s == 'L') + else if (*fmt == 'l' || *fmt == 'L') { lflag = 1; } - else if (*s >= '1' && *s <= '9') + else if (*fmt >= '1' && *fmt <= '9') { - for (tc = s; isdigit(*s); s++); - strncpy(tmp, tc, s - tc); - tmp[s - tc] = '\0'; + for (tc = fmt; isdigit(*fmt); fmt++); + strncpy(tmp, tc, fmt - tc); + tmp[fmt - tc] = '\0'; width = atoi(tmp); - s--; + fmt--; } } /* Process %s: String conversion */ - if (*s == 's') + if (*fmt == 's') { lvdbg("vsscanf: Performing string conversion\n"); @@ -178,9 +192,26 @@ int vsscanf(FAR char *buf, FAR const char *s, va_list ap) buf++; } + /* Was a fieldwidth specified? */ + if (!width) { - width = strcspn(buf, spaces); + /* No... is there a space after the format? */ + +#if 0 /* Needs more thought */ + if (isspace(*(s + 1)) || *(s + 1) == 0) +#endif + { + /* Use the input up until the first white space is + * encountered. NOTE: This means that values on the + * input line must be separated by whitespace or they + * will get combined! This is a bug. We have no good + * way of determining the width of the data if there + * is no field with and no space separating the input. + */ + + width = strcspn(buf, spaces); + } } if (!noassign) @@ -189,17 +220,22 @@ int vsscanf(FAR char *buf, FAR const char *s, va_list ap) strncpy(tv, buf, width); tv[width] = '\0'; } + buf += width; } /* Process %c: Character conversion */ - else if (*s == 'c') + else if (*fmt == 'c') { lvdbg("vsscanf: Performing character conversion\n"); + /* Was a fieldwidth specified? */ + if (!width) { + /* No, then width is this one single character */ + width = 1; } @@ -209,12 +245,13 @@ int vsscanf(FAR char *buf, FAR const char *s, va_list ap) strncpy(tv, buf, width); tv[width] = '\0'; } + buf += width; } /* Process %d, %o, %b, %x, %u: Various integer conversions */ - else if (strchr("dobxu", *s)) + else if (strchr("dobxu", *fmt)) { lvdbg("vsscanf: Performing integer conversion\n"); @@ -229,37 +266,47 @@ int vsscanf(FAR char *buf, FAR const char *s, va_list ap) * conversion specification. */ - if (*s == 'd' || *s == 'u') + if (*fmt == 'd' || *fmt == 'u') { base = 10; } - else if (*s == 'x') + else if (*fmt == 'x') { base = 16; } - else if (*s == 'o') + else if (*fmt == 'o') { base = 8; } - else if (*s == 'b') + else if (*fmt == 'b') { base = 2; } - /* Copy the integer string into a temporary working buffer. */ + /* Was a fieldwidth specified? */ if (!width) { + /* No... is there a space after the format? */ + +#if 0 /* Needs more thought */ if (isspace(*(s + 1)) || *(s + 1) == 0) +#endif { + /* Use the input up until the first white space is + * encountered. NOTE: This means that values on the + * input line must be separated by whitespace or they + * will get combined! This is a bug. We have no good + * way of determining the width of the data if there + * is no field with and no space separating the input. + */ + width = strcspn(buf, spaces); } - else - { - width = strchr(buf, *(s + 1)) - buf; - } } + /* Copy the numeric string into a temporary working buffer. */ + strncpy(tmp, buf, width); tmp[width] = '\0'; @@ -284,7 +331,7 @@ int vsscanf(FAR char *buf, FAR const char *s, va_list ap) /* Process %f: Floating point conversion */ - else if (*s == 'f') + else if (*fmt == 'f') { #ifndef CONFIG_LIBC_FLOATINGPOINT /* No floating point conversions */ @@ -303,20 +350,30 @@ int vsscanf(FAR char *buf, FAR const char *s, va_list ap) buf++; } - /* Copy the real string into a temporary working buffer. */ + /* Was a fieldwidth specified? */ if (!width) { + /* No... is there a space after the format? */ + +#if 0 /* Needs more thought */ if (isspace(*(s + 1)) || *(s + 1) == 0) +#endif { - width = strcspn(buf, spaces); - } - else - { - width = strchr(buf, *(s + 1)) - buf; + /* Use the input up until the first white space is + * encountered. NOTE: This means that values on the + * input line must be separated by whitespace or they + * will get combined! This is a bug. We have no good + * way of determining the width of the data if there + * is no field with and no space separating the input. + */ + + width = strcspn(buf, spaces); } } + /* Copy the real string into a temporary working buffer. */ + strncpy(tmp, buf, width); tmp[width] = '\0'; buf += width; @@ -356,13 +413,28 @@ int vsscanf(FAR char *buf, FAR const char *s, va_list ap) #endif } - if (!noassign) + /* Process %n: Character count */ + + else if (*fmt == 'n') + { + lvdbg("vsscanf: Performing character count\n"); + + if (!noassign) + { + int *pint = va_arg(ap, int*); + *pint = (int)(buf - bufstart) - 1; + } + } + + /* Note %n does not count as a conversion */ + + if (!noassign && *fmt != 'n') { count++; } width = noassign = lflag = 0; - s++; + fmt++; } /* Its is not a conversion specifier */ @@ -374,13 +446,13 @@ int vsscanf(FAR char *buf, FAR const char *s, va_list ap) buf++; } - if (*s != *buf) + if (*fmt != *buf) { break; } else { - s++; + fmt++; buf++; } } From 99bc5e453b0145f39528d73bae18fc5e6743cda4 Mon Sep 17 00:00:00 2001 From: patacongo Date: Fri, 21 Sep 2012 17:34:13 +0000 Subject: [PATCH 11/56] One more sscanf change git-svn-id: http://svn.code.sf.net/p/nuttx/code/trunk@5169 42af7a65-404d-4744-a932-0658087f49c3 --- nuttx/lib/stdio/lib_sscanf.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx/lib/stdio/lib_sscanf.c b/nuttx/lib/stdio/lib_sscanf.c index 11b5f387e8..a744c51d38 100644 --- a/nuttx/lib/stdio/lib_sscanf.c +++ b/nuttx/lib/stdio/lib_sscanf.c @@ -422,7 +422,7 @@ int vsscanf(FAR char *buf, FAR const char *fmt, va_list ap) if (!noassign) { int *pint = va_arg(ap, int*); - *pint = (int)(buf - bufstart) - 1; + *pint = (int)(buf - bufstart); } } From e8f1a8bff986a170bb03cd2f10bb08d90dab35f0 Mon Sep 17 00:00:00 2001 From: patacongo Date: Fri, 21 Sep 2012 17:55:08 +0000 Subject: [PATCH 12/56] Recent I2C changes for F4 broke F1 git-svn-id: http://svn.code.sf.net/p/nuttx/code/trunk@5170 42af7a65-404d-4744-a932-0658087f49c3 --- nuttx/arch/arm/src/stm32/stm32_i2c.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/nuttx/arch/arm/src/stm32/stm32_i2c.c b/nuttx/arch/arm/src/stm32/stm32_i2c.c index 12a036f806..c44a823dbb 100644 --- a/nuttx/arch/arm/src/stm32/stm32_i2c.c +++ b/nuttx/arch/arm/src/stm32/stm32_i2c.c @@ -1574,13 +1574,14 @@ static int stm32_i2c_process(FAR struct i2c_dev_s *dev, FAR struct i2c_msg_s *ms struct stm32_i2c_inst_s *inst = (struct stm32_i2c_inst_s *)dev; FAR struct stm32_i2c_priv_s *priv = inst->priv; uint32_t status = 0; + uint32_t ahbenr; int errval = 0; ASSERT(count); /* Disable FSMC that shares a pin with I2C1 (LBAR) */ - (void)stm32_i2c_disablefsmc(priv); + ahbenr = stm32_i2c_disablefsmc(priv); /* Wait for any STOP in progress. NOTE: If we have to disable the FSMC * then we cannot do this at the top of the loop, unfortunately. The STOP From 4a5f639e222c25405bbe0c381d7aa0880029761a Mon Sep 17 00:00:00 2001 From: patacongo Date: Fri, 21 Sep 2012 18:51:04 +0000 Subject: [PATCH 13/56] Fix bug in last sscanf change git-svn-id: http://svn.code.sf.net/p/nuttx/code/trunk@5171 42af7a65-404d-4744-a932-0658087f49c3 --- nuttx/lib/stdio/lib_sscanf.c | 109 ++++++++++++++++++++--------------- 1 file changed, 64 insertions(+), 45 deletions(-) diff --git a/nuttx/lib/stdio/lib_sscanf.c b/nuttx/lib/stdio/lib_sscanf.c index a744c51d38..032164ba1e 100644 --- a/nuttx/lib/stdio/lib_sscanf.c +++ b/nuttx/lib/stdio/lib_sscanf.c @@ -79,6 +79,64 @@ int vsscanf(char *buf, const char *fmt, va_list ap); static const char spaces[] = " \t\n\r\f\v"; +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Function: findwidth + * + * Description: + * Try to figure out the width of the input data. + * + ****************************************************************************/ + +static int findwidth(FAR const char *buf, FAR const char *fmt) +{ + FAR const char *next = fmt + 1; + + /* No... is there a space after the format? Or does the format string end + * here? + */ + + if (isspace(*next) || *next == 0) + { + /* Use the input up until the first white space is encountered. */ + + return strcspn(buf, spaces); + } + + /* No.. Another possibility is the the format character is followed by + * some recognizable delimiting value. + */ + + if (*next != '%') + { + /* If so we will say that the string ends there if we can find that + * delimiter in the input string. + */ + + FAR const char *ptr = strchr(buf, *next); + if (ptr) + { + return (int)(ptr - buf); + } + } + + /* No... the format has not delimiter and is back-to-back with the next + * formats (or no is following by a delimiter that does not exist in the + * input string). At this point we just bail and Use the input up until + * the first white space is encountered. + * + * NOTE: This means that values from the following format may be + * concatenated with the first. This is a bug. We have no generic way of + * determining the width of the data if there is no fieldwith, no space + * separating the input, and no usable delimiter character. + */ + + return strcspn(buf, spaces); +} + /**************************************************************************** * Private Variables ****************************************************************************/ @@ -196,22 +254,9 @@ int vsscanf(FAR char *buf, FAR const char *fmt, va_list ap) if (!width) { - /* No... is there a space after the format? */ + /* No... Guess a field width using some heuristics */ -#if 0 /* Needs more thought */ - if (isspace(*(s + 1)) || *(s + 1) == 0) -#endif - { - /* Use the input up until the first white space is - * encountered. NOTE: This means that values on the - * input line must be separated by whitespace or they - * will get combined! This is a bug. We have no good - * way of determining the width of the data if there - * is no field with and no space separating the input. - */ - - width = strcspn(buf, spaces); - } + width = findwidth(buf, fmt); } if (!noassign) @@ -287,22 +332,9 @@ int vsscanf(FAR char *buf, FAR const char *fmt, va_list ap) if (!width) { - /* No... is there a space after the format? */ + /* No... Guess a field width using some heuristics */ -#if 0 /* Needs more thought */ - if (isspace(*(s + 1)) || *(s + 1) == 0) -#endif - { - /* Use the input up until the first white space is - * encountered. NOTE: This means that values on the - * input line must be separated by whitespace or they - * will get combined! This is a bug. We have no good - * way of determining the width of the data if there - * is no field with and no space separating the input. - */ - - width = strcspn(buf, spaces); - } + width = findwidth(buf, fmt); } /* Copy the numeric string into a temporary working buffer. */ @@ -354,22 +386,9 @@ int vsscanf(FAR char *buf, FAR const char *fmt, va_list ap) if (!width) { - /* No... is there a space after the format? */ + /* No... Guess a field width using some heuristics */ -#if 0 /* Needs more thought */ - if (isspace(*(s + 1)) || *(s + 1) == 0) -#endif - { - /* Use the input up until the first white space is - * encountered. NOTE: This means that values on the - * input line must be separated by whitespace or they - * will get combined! This is a bug. We have no good - * way of determining the width of the data if there - * is no field with and no space separating the input. - */ - - width = strcspn(buf, spaces); - } + width = findwidth(buf, fmt); } /* Copy the real string into a temporary working buffer. */ From 6052cd078a07e5f3c4f7481537f2b17c48d32d98 Mon Sep 17 00:00:00 2001 From: patacongo Date: Fri, 21 Sep 2012 22:05:41 +0000 Subject: [PATCH 14/56] More sscanf: Long flag (as in %ld) not be used in all of the places it should be git-svn-id: http://svn.code.sf.net/p/nuttx/code/trunk@5172 42af7a65-404d-4744-a932-0658087f49c3 --- nuttx/lib/stdio/lib_sscanf.c | 54 ++++++++++++++++++++++++++---------- 1 file changed, 40 insertions(+), 14 deletions(-) diff --git a/nuttx/lib/stdio/lib_sscanf.c b/nuttx/lib/stdio/lib_sscanf.c index 032164ba1e..7e1fae276d 100644 --- a/nuttx/lib/stdio/lib_sscanf.c +++ b/nuttx/lib/stdio/lib_sscanf.c @@ -38,9 +38,11 @@ ****************************************************************************/ #include + #include #include #include +#include #include #include #include @@ -173,11 +175,11 @@ int vsscanf(FAR char *buf, FAR const char *fmt, va_list ap) FAR char *bufstart; FAR char *tv; FAR const char *tc; + bool lflag; + bool noassign; int count; - int noassign; int width; int base = 10; - int lflag; char tmp[MAXLN]; lvdbg("vsscanf: buf=\"%s\" fmt=\"%s\"\n", buf, fmt); @@ -191,9 +193,9 @@ int vsscanf(FAR char *buf, FAR const char *fmt, va_list ap) /* Parse the format, extracting values from the input buffer as needed */ count = 0; - noassign = 0; width = 0; - lflag = 0; + noassign = false; + lflag = false; while (*fmt && *buf) { @@ -223,11 +225,13 @@ int vsscanf(FAR char *buf, FAR const char *fmt, va_list ap) if (*fmt == '*') { - noassign = 1; + noassign = true; } else if (*fmt == 'l' || *fmt == 'L') { - lflag = 1; + /* NOTE: Missing check for long long ('ll') */ + + lflag = true; } else if (*fmt >= '1' && *fmt <= '9') { @@ -349,15 +353,24 @@ int vsscanf(FAR char *buf, FAR const char *fmt, va_list ap) buf += width; if (!noassign) { - int *pint = va_arg(ap, int*); #ifdef SDCC char *endptr; - int tmpint = strtol(tmp, &endptr, base); + long tmplong = strtol(tmp, &endptr, base); #else - int tmpint = strtol(tmp, NULL, base); + long tmplong = strtol(tmp, NULL, base); #endif - lvdbg("vsscanf: Return %d to 0x%p\n", tmpint, pint); - *pint = tmpint; + if (lflag) + { + long *plong = va_arg(ap, long*); + lvdbg("vsscanf: Return %ld to 0x%p\n", tmplong, plong); + *plong = tmplong; + } + else + { + int *pint = va_arg(ap, int*); + lvdbg("vsscanf: Return %ld to 0x%p\n", tmplong, pint); + *pint = (int)tmplong; + } } } @@ -440,8 +453,18 @@ int vsscanf(FAR char *buf, FAR const char *fmt, va_list ap) if (!noassign) { - int *pint = va_arg(ap, int*); - *pint = (int)(buf - bufstart); + size_t nchars = (size_t)(buf - bufstart); + + if (lflag) + { + long *plong = va_arg(ap, long*); + *plong = (long)nchars; + } + else + { + int *pint = va_arg(ap, int*); + *pint = (int)nchars; + } } } @@ -452,7 +475,10 @@ int vsscanf(FAR char *buf, FAR const char *fmt, va_list ap) count++; } - width = noassign = lflag = 0; + width = 0; + noassign = false; + lflag = false; + fmt++; } From 1b7786e0e26cadbad49812f5fd97650e18cd7378 Mon Sep 17 00:00:00 2001 From: patacongo Date: Sat, 22 Sep 2012 12:23:35 +0000 Subject: [PATCH 15/56] More webserver updates from Kate git-svn-id: http://svn.code.sf.net/p/nuttx/code/trunk@5173 42af7a65-404d-4744-a932-0658087f49c3 --- apps/ChangeLog.txt | 2 + apps/netutils/webserver/httpd.c | 221 ++++++++++++++++++++++++-------- 2 files changed, 167 insertions(+), 56 deletions(-) diff --git a/apps/ChangeLog.txt b/apps/ChangeLog.txt index e098099155..c5b8dec92d 100644 --- a/apps/ChangeLog.txt +++ b/apps/ChangeLog.txt @@ -336,3 +336,5 @@ can now be used to limit the server to a single thread. Option CONFIG_NETUTILS_HTTPD_TIMEOUT can be used to generate HTTP 408 errors. Both from Kate. + * apps/netutils/webserver/httpd.c: Improvements to HTTP parser from + Kate. diff --git a/apps/netutils/webserver/httpd.c b/apps/netutils/webserver/httpd.c index 29438c77af..ebcf300b68 100644 --- a/apps/netutils/webserver/httpd.c +++ b/apps/netutils/webserver/httpd.c @@ -118,8 +118,6 @@ * Private Data ****************************************************************************/ -static const char g_httpcmdget[] = "GET "; - /**************************************************************************** * Private Functions ****************************************************************************/ @@ -179,7 +177,7 @@ static void httpd_dumpbuffer(FAR const char *msg, FAR const char *buffer, unsign /* CONFIG_DEBUG, CONFIG_DEBUG_VERBOSE, and CONFIG_DEBUG_NET have to be * defined or the following does nothing. */ - + nvdbgdumpbuffer(msg, (FAR const uint8_t*)buffer, nbytes); } #else @@ -194,8 +192,10 @@ static void httpd_dumppstate(struct httpd_state *pstate, const char *msg) nvdbg(" filename: [%s]\n", pstate->ht_filename); nvdbg(" htfile len: %d\n", pstate->ht_file.len); nvdbg(" sockfd: %d\n", pstate->ht_sockfd); +#ifndef CONFIG_NETUTILS_HTTPD_SCRIPT_DISABLE nvdbg(" scriptptr: %p\n", pstate->ht_scriptptr); nvdbg(" scriptlen: %d\n", pstate->ht_scriptlen); +#endif nvdbg(" sndlen: %d\n", pstate->ht_sndlen); #endif } @@ -373,6 +373,11 @@ static int send_headers(struct httpd_state *pstate, int status, int len) (void) snprintf(cl, sizeof cl, "Content-Length: %d\r\n", len); } + if (status == 413) + { + /* TODO: here we "SHOULD" include a Retry-After header */ + } + i = snprintf(s, sizeof s, "HTTP/1.0 %d %s\r\n" #ifndef CONFIG_NETUTILS_HTTPD_SERVERHEADER_DISABLE @@ -498,73 +503,167 @@ done: return ret; } -static inline int httpd_cmd(struct httpd_state *pstate) +static inline int httpd_parse(struct httpd_state *pstate) { - ssize_t recvlen; - int i; + char *o; - /* Get the next HTTP command. We will handle only GET */ + enum + { + STATE_METHOD, + STATE_HEADER, + STATE_BODY + } state; + + state = STATE_METHOD; + o = pstate->ht_buffer; + + do + { + char *start; + char *end; + + if (o == pstate->ht_buffer + sizeof pstate->ht_buffer) + { + ndbg("[%d] ht_buffer overflow\n"); + return 413; + } + + { + ssize_t r; + + r = recv(pstate->ht_sockfd, o, + sizeof pstate->ht_buffer - (o - pstate->ht_buffer), 0); + if (r == 0) + { + ndbg("[%d] connection lost\n", pstate->ht_sockfd); + return ERROR; + } - recvlen = recv(pstate->ht_sockfd, pstate->ht_buffer, HTTPD_IOBUFFER_SIZE, 0); #if CONFIG_NETUTILS_HTTPD_TIMEOUT > 0 - if (recvlen < 0 && errno == EWOULDBLOCK) - { - ndbg("[%d] recv timeout\n"); - return httpd_senderror(pstate, 408); - } - else + if (r == -1 && errno == EWOULDBLOCK) + { + ndbg("[%d] recv timeout\n"); + return 408; + } #endif - if (recvlen < 0) - { - ndbg("[%d] recv failed: %d\n", pstate->ht_sockfd, errno); - return httpd_senderror(pstate, 400); - } - else if (recvlen == 0) - { - ndbg("[%d] connection lost\n", pstate->ht_sockfd); - return httpd_senderror(pstate, 400); + if (r == -1) + { + ndbg("[%d] recv failed: %d\n", pstate->ht_sockfd, errno); + return 400; + } + + o += r; + } + + /* Here o marks the end of the total block currently awaiting processing. + * There may be multiple lines in a block; next we deal with each in turn. + */ + + for (start = pstate->ht_buffer; + (end = memchr(start, '\r', o - start)), end != NULL; + start = end) + { + *end = '\0'; + end++; + + /* Here start and end are a single line within the current block */ + + httpd_dumpbuffer("Incoming HTTP line", start, end - start); + + if (*end != '\n') + { + ndbg("[%d] expected CRLF\n"); + return 400; + } + + end++; + + switch (state) + { + char *v; + + case STATE_METHOD: + if (0 != strncmp(start, "GET ", 4)) + { + ndbg("[%d] method not supported\n"); + return 501; + } + + start += 4; + v = start + strcspn(start, " "); + + if (0 != strcmp(v, " HTTP/1.0") && 0 != strcmp(v, " HTTP/1.1")) + { + ndbg("[%d] HTTP/%d.%d not supported\n", major, minor); + return 505; + } + + /* TODO: url decoding */ + + if (v - start >= sizeof pstate->ht_filename) + { + ndbg("[%d] ht_filename overflow\n"); + return 414; + } + + *v = '\0'; + (void) strcpy(pstate->ht_filename, start); + state = STATE_HEADER; + break; + + case STATE_HEADER: + if (*start == '\0') + { + state = STATE_BODY; + break; + } + + v = start + strcspn(start, ":"); + if (*v != '\0') + { + *v = '\0', v++; + v += strspn(v, ": "); + } + + if (*start == '\0' || *v == '\0') + { + ndbg("[%d] header parse error\n"); + return 400; + } + + nvdbg("[%d] Request header %s: %s\n", pstate->ht_sockfd, start, v); + + if (0 == strcasecmp(start, "Content-Length") && 0 != atoi(v)) + { + ndbg("[%d] non-zero request length\n"); + return 413; + } + + break; + + case STATE_BODY: + /* Not implemented */ + break; + } + } + + /* Shuffle down for the next block */ + + memmove(pstate->ht_buffer, start, o - start); + o -= start; } + while (state != STATE_BODY); - httpd_dumpbuffer("Incoming buffer", pstate->ht_buffer, recvlen); - - /* We will handle only GET */ - - if (strncmp(pstate->ht_buffer, g_httpcmdget, strlen(g_httpcmdget)) != 0) - { - ndbg("[%d] Unsupported command\n", pstate->ht_sockfd); - return httpd_senderror(pstate, 405); - } - - /* Get the name of the file to provide */ - - if (pstate->ht_buffer[4] != ISO_slash) - { - ndbg("[%d] Missing path\n", pstate->ht_sockfd); - return httpd_senderror(pstate, 400); - } #if !defined(CONFIG_NETUTILS_HTTPD_SENDFILE) && !defined(CONFIG_NETUTILS_HTTPD_MMAP) - else if (pstate->ht_buffer[5] == ISO_space) + if (0 == strcmp(pstate->ht_filename, "/") { strncpy(pstate->ht_filename, "/" CONFIG_NETUTILS_HTTPD_INDEX, strlen("/" CONFIG_NETUTILS_HTTPD_INDEX)); } #endif - else - { - for (i = 0; - i < (HTTPD_MAX_FILENAME-1) && pstate->ht_buffer[i+4] != ISO_space; - i++) - { - pstate->ht_filename[i] = pstate->ht_buffer[i+4]; - } - - pstate->ht_filename[i]='\0'; - } nvdbg("[%d] Filename: %s\n", pstate->ht_sockfd, pstate->ht_filename); - /* Then send the file */ - - return httpd_sendfile(pstate); + return 200; } /**************************************************************************** @@ -589,6 +688,8 @@ static void *httpd_handler(void *arg) if (pstate) { + int status; + /* Re-initialize the thread state structure */ memset(pstate, 0, sizeof(struct httpd_state)); @@ -596,7 +697,15 @@ static void *httpd_handler(void *arg) /* Then handle the next httpd command */ - ret = httpd_cmd(pstate); + status = httpd_parse(pstate); + if (status >= 400) + { + ret = httpd_senderror(pstate, status); + } + else + { + ret = httpd_sendfile(pstate); + } /* End of command processing -- Clean up and exit */ From 4f381f2185dcf6f2e6d302538f82a9ccb5f1b106 Mon Sep 17 00:00:00 2001 From: patacongo Date: Sat, 22 Sep 2012 14:06:47 +0000 Subject: [PATCH 16/56] Correct Shenzhou LED controls git-svn-id: http://svn.code.sf.net/p/nuttx/code/trunk@5174 42af7a65-404d-4744-a932-0658087f49c3 --- nuttx/configs/shenzhou/README.txt | 6 +- nuttx/configs/shenzhou/nsh/defconfig | 119 ++++++++++-------- nuttx/configs/shenzhou/scripts/ld.script | 12 +- nuttx/configs/shenzhou/scripts/ld.script.dfu | 12 +- .../configs/shenzhou/src/shenzhou-internal.h | 8 +- nuttx/configs/shenzhou/src/up_autoleds.c | 20 +-- 6 files changed, 101 insertions(+), 76 deletions(-) diff --git a/nuttx/configs/shenzhou/README.txt b/nuttx/configs/shenzhou/README.txt index 9ee77ea06f..e68bd7bf9f 100644 --- a/nuttx/configs/shenzhou/README.txt +++ b/nuttx/configs/shenzhou/README.txt @@ -381,12 +381,12 @@ events as follows: * If LED1, LED2, LED3 are statically on, then NuttX probably failed to boot and these LEDs will give you some indication of where the failure was - ** The normal state is LED3 ON and LED1 faintly glowing. This faint glow + ** The normal state is LED1 ON and LED1 faintly glowing. This faint glow is because of timer interupts that result in the LED being illuminated on a small proportion of the time. *** LED2 may also flicker normally if signals are processed. -**** LED4 may not be available if RS-485 is also used it will then indicate - the RS-485 direction. +**** LED4 may not be available if RS-485 is also used. For RS-485, it will + then indicate the RS-485 direction. Shenzhou-specific Configuration Options ============================================ diff --git a/nuttx/configs/shenzhou/nsh/defconfig b/nuttx/configs/shenzhou/nsh/defconfig index 0c18b129f2..70d9540db8 100644 --- a/nuttx/configs/shenzhou/nsh/defconfig +++ b/nuttx/configs/shenzhou/nsh/defconfig @@ -175,6 +175,7 @@ CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG=y CONFIG_STM32_PHYADDR=1 CONFIG_STM32_MII=y CONFIG_STM32_MII_MCO=y +# CONFIG_STM32_MII_EXTCLK is not set CONFIG_STM32_AUTONEG=y CONFIG_STM32_PHYSR=16 CONFIG_STM32_PHYSR_SPEED=0x0002 @@ -200,7 +201,7 @@ CONFIG_ARCH_STACKDUMP=y CONFIG_DRAM_START=0x20000000 CONFIG_DRAM_SIZE=65536 CONFIG_ARCH_HAVE_INTERRUPTSTACK=y -# CONFIG_ARCH_INTERRUPTSTACK is not set +CONFIG_ARCH_INTERRUPTSTACK=0 # # Boot options @@ -307,6 +308,7 @@ CONFIG_DEV_NULL=y # CONFIG_CAN is not set # CONFIG_PWM is not set # CONFIG_I2C is not set +CONFIG_ARCH_HAVE_I2CRESET=y CONFIG_SPI=y # CONFIG_SPI_OWNBUS is not set CONFIG_SPI_EXCHANGE=y @@ -468,142 +470,142 @@ CONFIG_NAMEDAPP=y # # -# ADC example +# ADC Example # # CONFIG_EXAMPLES_ADC is not set # -# Buttons example +# Buttons Example # # CONFIG_EXAMPLES_BUTTONS is not set # -# CAN example +# CAN Example # # CONFIG_EXAMPLES_CAN is not set # -# USB CDC/ACM class driver example +# USB CDC/ACM Class Driver Example # # CONFIG_EXAMPLES_CDCACM is not set # -# USB composite class driver example +# USB composite Class Driver Example # # CONFIG_EXAMPLES_COMPOSITE is not set # -# DHCP server example +# DHCP Server Example # # CONFIG_EXAMPLES_DHCPD is not set # -# FTP client example +# FTP Client Example # # CONFIG_EXAMPLES_FTPC is not set # -# FTP server example +# FTP Server Example # # CONFIG_EXAMPLES_FTPD is not set # -# "Hello, World!" example +# "Hello, World!" Example # # CONFIG_EXAMPLES_HELLO is not set # -# "Hello, World!" C++ example +# "Hello, World!" C++ Example # # CONFIG_EXAMPLES_HELLOXX is not set # -# USB HID keyboard example +# USB HID Keyboard Example # # CONFIG_EXAMPLES_HIDKBD is not set # -# IGMP example +# IGMP Example # # CONFIG_EXAMPLES_IGMP is not set # -# LCD read/write example +# LCD Read/Write Example # # CONFIG_EXAMPLES_LCDRW is not set # -# Memory management example +# Memory Management Example # # CONFIG_EXAMPLES_MM is not set # -# File system mount example +# File System Mount Example # # CONFIG_EXAMPLES_MOUNT is not set # -# FreeModBus example +# FreeModBus Example # # CONFIG_EXAMPLES_MODBUS is not set # -# Network test example +# Network Test Example # # CONFIG_EXAMPLES_NETTEST is not set # -# NuttShell (NSH) example +# NuttShell (NSH) Example # CONFIG_EXAMPLES_NSH=y # -# NULL example +# NULL Example # # CONFIG_EXAMPLES_NULL is not set # -# NX graphics example +# NX Graphics Example # # CONFIG_EXAMPLES_NX is not set # -# NxConsole example +# NxConsole Example # # CONFIG_EXAMPLES_NXCONSOLE is not set # -# NXFFS file system example +# NXFFS File System Example # # CONFIG_EXAMPLES_NXFFS is not set # -# NXFLAT example +# NXFLAT Example # # CONFIG_EXAMPLES_NXFLAT is not set # -# NX graphics "Hello, World!" example +# NX Graphics "Hello, World!" Example # # CONFIG_EXAMPLES_NXHELLO is not set # -# NX graphics image example +# NX Graphics image Example # # CONFIG_EXAMPLES_NXIMAGE is not set # -# NX graphics lines example +# NX Graphics lines Example # # CONFIG_EXAMPLES_NXLINES is not set # -# NX graphics text example +# NX Graphics Text Example # # CONFIG_EXAMPLES_NXTEXT is not set # -# OS test example +# OS Test Example # # CONFIG_EXAMPLES_OSTEST is not set @@ -613,104 +615,113 @@ CONFIG_EXAMPLES_NSH=y # CONFIG_EXAMPLES_PASHELLO is not set # -# Pipe example +# Pipe Example # # CONFIG_EXAMPLES_PIPE is not set # -# Poll example +# Poll Example # # CONFIG_EXAMPLES_POLL is not set # -# Pulse width modulation (PWM) example +# Pulse Width Modulation (PWM) Example # # -# Quadrature encoder example +# Quadrature Encoder Example # # CONFIG_EXAMPLES_QENCODER is not set # -# RGMP example +# RGMP Example # # CONFIG_EXAMPLES_RGMP is not set # -# ROMFS example +# ROMFS Example # # CONFIG_EXAMPLES_ROMFS is not set # -# sendmail example +# sendmail Example # # CONFIG_EXAMPLES_SENDMAIL is not set # -# Serial loopback example +# Serial Loopback Example # # CONFIG_EXAMPLES_SERLOOP is not set # -# Telnet daemon example +# Telnet Daemon Example # # CONFIG_EXAMPLES_TELNETD is not set # -# THTTPD web server example +# THTTPD Web Server Example # # CONFIG_EXAMPLES_THTTPD is not set # -# TIFF generation example +# TIFF Generation Example # # CONFIG_EXAMPLES_TIFF is not set # -# Touchscreen example +# Touchscreen Example # # CONFIG_EXAMPLES_TOUCHSCREEN is not set # -# UDP example +# UDP Example # # CONFIG_EXAMPLES_UDP is not set # -# uIP web server example +# UDP Discovery Daemon Example +# +# CONFIG_EXAMPLE_DISCOVER is not set + +# +# uIP Web Server Example # # CONFIG_EXAMPLES_UIP is not set # -# USB serial test example +# USB Serial Test Example # # CONFIG_EXAMPLES_USBSERIAL is not set # -# USB mass storage class example +# USB Mass Storage Class Example # # CONFIG_EXAMPLES_USBMSC is not set # -# USB serial terminal example +# USB Serial Terminal Example # # CONFIG_EXAMPLES_USBTERM is not set # -# Watchdog timer example +# Watchdog timer Example # # CONFIG_EXAMPLES_WATCHDOG is not set # -# wget example +# wget Example # # CONFIG_EXAMPLES_WGET is not set # -# WLAN example +# WLAN Example # # CONFIG_EXAMPLES_WLAN is not set +# +# XML RPC Example +# + # # Interpreters # @@ -790,6 +801,16 @@ CONFIG_NETUTILS_WEBCLIENT=y # # CONFIG_NETUTILS_WEBSERVER is not set +# +# UDP Discovery Utility +# +# CONFIG_NETUTILS_DISCOVER is not set + +# +# XML-RPC library +# +# CONFIG_NETUTILS_XMLRPC is not set + # # ModBus # diff --git a/nuttx/configs/shenzhou/scripts/ld.script b/nuttx/configs/shenzhou/scripts/ld.script index 14f924baec..f07d4432d1 100644 --- a/nuttx/configs/shenzhou/scripts/ld.script +++ b/nuttx/configs/shenzhou/scripts/ld.script @@ -63,6 +63,12 @@ SECTIONS _etext = ABSOLUTE(.); } > flash + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } >flash + __exidx_end = ABSOLUTE(.); + _eronly = ABSOLUTE(.); /* The STM32F107VC has 64Kb of SRAM beginning at the following address */ @@ -79,12 +85,6 @@ SECTIONS *(.ARM.extab*) } >sram - __exidx_start = ABSOLUTE(.); - .ARM.exidx : { - *(.ARM.exidx*) - } >sram - __exidx_end = ABSOLUTE(.); - .bss : { _sbss = ABSOLUTE(.); *(.bss .bss.*) diff --git a/nuttx/configs/shenzhou/scripts/ld.script.dfu b/nuttx/configs/shenzhou/scripts/ld.script.dfu index 53e3ee4fca..b9d94c7a0f 100644 --- a/nuttx/configs/shenzhou/scripts/ld.script.dfu +++ b/nuttx/configs/shenzhou/scripts/ld.script.dfu @@ -65,6 +65,12 @@ SECTIONS _etext = ABSOLUTE(.); } > flash + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } >flash + __exidx_end = ABSOLUTE(.); + _eronly = ABSOLUTE(.); /* The STM32F107VC has 64Kb of SRAM beginning at the following address */ @@ -81,12 +87,6 @@ SECTIONS *(.ARM.extab*) } >sram - __exidx_start = ABSOLUTE(.); - .ARM.exidx : { - *(.ARM.exidx*) - } >sram - __exidx_end = ABSOLUTE(.); - .bss : { _sbss = ABSOLUTE(.); *(.bss .bss.*) diff --git a/nuttx/configs/shenzhou/src/shenzhou-internal.h b/nuttx/configs/shenzhou/src/shenzhou-internal.h index 19460bc998..48b06e7b0a 100644 --- a/nuttx/configs/shenzhou/src/shenzhou-internal.h +++ b/nuttx/configs/shenzhou/src/shenzhou-internal.h @@ -113,13 +113,13 @@ */ #define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ - GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN2) + GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN2) #define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ - GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN3) + GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN3) #define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ - GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN4) + GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN4) #define GPIO_LED4 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ - GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN5) + GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7) /* TFT LCD * diff --git a/nuttx/configs/shenzhou/src/up_autoleds.c b/nuttx/configs/shenzhou/src/up_autoleds.c index 996836c792..3c467c8ad7 100644 --- a/nuttx/configs/shenzhou/src/up_autoleds.c +++ b/nuttx/configs/shenzhou/src/up_autoleds.c @@ -203,24 +203,26 @@ static struct pm_callback_s g_ledscb = static inline void led_clrbits(unsigned int clrbits) { + /* All LEDs are pulled up and, hence, active low */ + if ((clrbits & SHENZHOU_LED1) != 0) { - stm32_gpiowrite(GPIO_LED1, false); + stm32_gpiowrite(GPIO_LED1, true); } if ((clrbits & SHENZHOU_LED2) != 0) { - stm32_gpiowrite(GPIO_LED2, false); + stm32_gpiowrite(GPIO_LED2, true); } if ((clrbits & SHENZHOU_LED3) != 0) { - stm32_gpiowrite(GPIO_LED3, false); + stm32_gpiowrite(GPIO_LED3, true); } if ((clrbits & SHENZHOU_LED4) != 0) { - stm32_gpiowrite(GPIO_LED4, false); + stm32_gpiowrite(GPIO_LED4, true); } } @@ -234,24 +236,26 @@ static inline void led_clrbits(unsigned int clrbits) static inline void led_setbits(unsigned int setbits) { + /* All LEDs are pulled up and, hence, active low */ + if ((setbits & SHENZHOU_LED1) != 0) { - stm32_gpiowrite(GPIO_LED1, true); + stm32_gpiowrite(GPIO_LED1, false); } if ((setbits & SHENZHOU_LED2) != 0) { - stm32_gpiowrite(GPIO_LED2, true); + stm32_gpiowrite(GPIO_LED2, false); } if ((setbits & SHENZHOU_LED3) != 0) { - stm32_gpiowrite(GPIO_LED3, true); + stm32_gpiowrite(GPIO_LED3, false); } if ((setbits & SHENZHOU_LED4) != 0) { - stm32_gpiowrite(GPIO_LED4, true); + stm32_gpiowrite(GPIO_LED4, false); } } From fc8f6c972a15241790d9443f33b2e97d4347c3c8 Mon Sep 17 00:00:00 2001 From: patacongo Date: Sat, 22 Sep 2012 15:12:50 +0000 Subject: [PATCH 17/56] Make the Olimex stm32 p107 clock configuratin the standard for connectivity line devices git-svn-id: http://svn.code.sf.net/p/nuttx/code/trunk@5175 42af7a65-404d-4744-a932-0658087f49c3 --- nuttx/arch/arm/src/stm32/Kconfig | 4 +- nuttx/arch/arm/src/stm32/stm32f10xxx_rcc.c | 127 ++++++++++++- .../configs/olimex-stm32-p107/include/board.h | 32 ++-- nuttx/configs/olimex-stm32-p107/nsh/defconfig | 1 - .../olimex-stm32-p107/ostest/defconfig | 1 - nuttx/configs/olimex-stm32-p107/src/up_boot.c | 108 ----------- nuttx/configs/shenzhou/include/board.h | 32 ++-- nuttx/configs/shenzhou/nsh/defconfig | 2 +- nuttx/configs/shenzhou/src/Makefile | 4 - nuttx/configs/shenzhou/src/up_clockconfig.c | 167 ------------------ 10 files changed, 158 insertions(+), 320 deletions(-) delete mode 100644 nuttx/configs/shenzhou/src/up_clockconfig.c diff --git a/nuttx/arch/arm/src/stm32/Kconfig b/nuttx/arch/arm/src/stm32/Kconfig index a0f9a48921..c7dae44608 100644 --- a/nuttx/arch/arm/src/stm32/Kconfig +++ b/nuttx/arch/arm/src/stm32/Kconfig @@ -1732,11 +1732,11 @@ config STM32_MII_EXTCLK endchoice config STM32_AUTONEG - bool "Use autonegtiation" + bool "Use autonegotiation" default y depends on STM32_ETHMAC ---help--- - Use PHY autonegotion to determine speed and mode + Use PHY autonegotiation to determine speed and mode config STM32_ETHFD bool "Full duplex" diff --git a/nuttx/arch/arm/src/stm32/stm32f10xxx_rcc.c b/nuttx/arch/arm/src/stm32/stm32f10xxx_rcc.c index 10b8572cf0..47ed5e016b 100644 --- a/nuttx/arch/arm/src/stm32/stm32f10xxx_rcc.c +++ b/nuttx/arch/arm/src/stm32/stm32f10xxx_rcc.c @@ -90,7 +90,7 @@ static inline void rcc_reset(void) regval = getreg32(STM32_RCC_CR); /* Reset HSEBYP bit */ regval &= ~RCC_CR_HSEBYP; putreg32(regval, STM32_RCC_CR); - + regval = getreg32(STM32_RCC_CFGR); /* Reset PLLSRC, PLLXTPRE, PLLMUL and USBPRE bits */ regval &= ~(RCC_CFGR_PLLSRC|RCC_CFGR_PLLXTPRE|RCC_CFGR_PLLMUL_MASK|RCC_CFGR_USBPRE); putreg32(regval, STM32_RCC_CFGR); @@ -235,7 +235,7 @@ static inline void rcc_enableapb1(void) regval |= RCC_APB1ENR_SPI2EN; #endif - + #ifdef CONFIG_STM32_SPI3 /* SPI 3 clock enable */ @@ -411,13 +411,128 @@ static inline void rcc_enableapb2(void) * Name: stm32_stdclockconfig * * Description: - * Called to change to new clock based on settings in board.h - * + * Called to change to new clock based on settings in board.h. This + * version is for the Connectivity Line parts. + * * NOTE: This logic would need to be extended if you need to select low- * power clocking modes! ****************************************************************************/ -#ifndef CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG +#if !defined(CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG) && defined(CONFIG_STM32_CONNECTIVITYLINE) +static void stm32_stdclockconfig(void) +{ + uint32_t regval; + + /* Enable HSE */ + + regval = getreg32(STM32_RCC_CR); + regval &= ~RCC_CR_HSEBYP; /* Disable HSE clock bypass */ + regval |= RCC_CR_HSEON; /* Enable HSE */ + putreg32(regval, STM32_RCC_CR); + + /* Set flash wait states + * Sysclk runs with 72MHz -> 2 waitstates. + * 0WS from 0-24MHz + * 1WS from 24-48MHz + * 2WS from 48-72MHz + */ + + regval = getreg32(STM32_FLASH_ACR); + regval &= ~FLASH_ACR_LATENCY_MASK; + regval |= (FLASH_ACR_LATENCY_2|FLASH_ACR_PRTFBE); + putreg32(regval, STM32_FLASH_ACR); + + /* Set up PLL input scaling (with source = PLL2) */ + + regval = getreg32(STM32_RCC_CFGR2); + regval &= ~(RCC_CFGR2_PREDIV2_MASK | RCC_CFGR2_PLL2MUL_MASK | + RCC_CFGR2_PREDIV1SRC_MASK | RCC_CFGR2_PREDIV1_MASK); + regval |= (STM32_PLL_PREDIV2 | STM32_PLL_PLL2MUL | + RCC_CFGR2_PREDIV1SRC_PLL2 | STM32_PLL_PREDIV1); + putreg32(regval, STM32_RCC_CFGR2); + + /* Set the PCLK2 divider */ + + regval = getreg32(STM32_RCC_CFGR); + regval &= ~(RCC_CFGR_PPRE2_MASK | RCC_CFGR_HPRE_MASK); + regval |= STM32_RCC_CFGR_PPRE2; + regval |= RCC_CFGR_HPRE_SYSCLK; + putreg32(regval, STM32_RCC_CFGR); + + /* Set the PCLK1 divider */ + + regval = getreg32(STM32_RCC_CFGR); + regval &= ~RCC_CFGR_PPRE1_MASK; + regval |= STM32_RCC_CFGR_PPRE1; + putreg32(regval, STM32_RCC_CFGR); + + /* Enable PLL2 */ + + regval = getreg32(STM32_RCC_CR); + regval |= RCC_CR_PLL2ON; + putreg32(regval, STM32_RCC_CR); + + /* Wait for PLL2 ready */ + + while((getreg32(STM32_RCC_CR) & RCC_CR_PLL2RDY) == 0); + + /* Setup PLL3 for MII/RMII clock on MCO */ + +#if defined(CONFIG_STM32_MII_MCO) || defined(CONFIG_STM32_RMII_MCO) + regval = getreg32(STM32_RCC_CFGR2); + regval &= ~(RCC_CFGR2_PLL3MUL_MASK); + regval |= STM32_PLL_PLL3MUL; + putreg32(regval, STM32_RCC_CFGR2); + + /* Switch PLL3 on */ + + regval = getreg32(STM32_RCC_CR); + regval |= RCC_CR_PLL3ON; + putreg32(regval, STM32_RCC_CR); + + while ((getreg32(STM32_RCC_CR) & RCC_CR_PLL3RDY) == 0); +#endif + + /* Set main PLL source and multiplier */ + + regval = getreg32(STM32_RCC_CFGR); + regval &= ~(RCC_CFGR_PLLSRC | RCC_CFGR_PLLMUL_MASK); + regval |= (RCC_CFGR_PLLSRC | STM32_PLL_PLLMUL); + putreg32(regval, STM32_RCC_CFGR); + + /* Switch main PLL on */ + + regval = getreg32(STM32_RCC_CR); + regval |= RCC_CR_PLLON; + putreg32(regval, STM32_RCC_CR); + + while ((getreg32(STM32_RCC_CR) & RCC_CR_PLLRDY) == 0); + + /* Select PLL as system clock source */ + + regval = getreg32(STM32_RCC_CFGR); + regval &= ~RCC_CFGR_SW_MASK; + regval |= RCC_CFGR_SW_PLL; + putreg32(regval, STM32_RCC_CFGR); + + /* Wait until PLL is used as the system clock source */ + + while ((getreg32(STM32_RCC_CFGR) & RCC_CFGR_SWS_PLL) == 0); +} +#endif + +/**************************************************************************** + * Name: stm32_stdclockconfig + * + * Description: + * Called to change to new clock based on settings in board.h. This + * version is for the non-Connectivity Line parts. + * + * NOTE: This logic would need to be extended if you need to select low- + * power clocking modes! + ****************************************************************************/ + +#if !defined(CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG) && !defined(CONFIG_STM32_CONNECTIVITYLINE) static void stm32_stdclockconfig(void) { uint32_t regval; @@ -430,7 +545,7 @@ static void stm32_stdclockconfig(void) volatile int32_t timeout; /* Enable External High-Speed Clock (HSE) */ - + regval = getreg32(STM32_RCC_CR); regval &= ~RCC_CR_HSEBYP; /* Disable HSE clock bypass */ regval |= RCC_CR_HSEON; /* Enable HSE */ diff --git a/nuttx/configs/olimex-stm32-p107/include/board.h b/nuttx/configs/olimex-stm32-p107/include/board.h index 2fe6e5aafe..d531eb1129 100644 --- a/nuttx/configs/olimex-stm32-p107/include/board.h +++ b/nuttx/configs/olimex-stm32-p107/include/board.h @@ -57,9 +57,18 @@ /* On-board crystal frequency is 25MHz (HSE) */ #define STM32_BOARD_XTAL 25000000ul -#define STM32_PLL_FREQUENCY (72000000) -#define STM32_SYSCLK_FREQUENCY STM32_PLL_FREQUENCY +/* PLL ouput is 72MHz */ + +#define STM32_PLL_PREDIV2 RCC_CFGR2_PREDIV2d5 /* 25MHz / 5 => 5MHz */ +#define STM32_PLL_PLL2MUL RCC_CFGR2_PLL2MULx8 /* 5MHz * 8 => 40MHz */ +#define STM32_PLL_PREDIV1 RCC_CFGR2_PREDIV1d5 /* 40MHz / 5 => 8MHz */ +#define STM32_PLL_PLLMUL RCC_CFGR_PLLMUL_CLKx9 /* 8MHz * 9 => 72Mhz */ +#define STM32_PLL_FREQUENCY (72000000) + +/* SYCLLK and HCLK are the PLL frequency */ + +#define STM32_SYSCLK_FREQUENCY STM32_PLL_FREQUENCY #define STM32_HCLK_FREQUENCY STM32_PLL_FREQUENCY #define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */ @@ -88,6 +97,12 @@ #define STM32_APB1_TIM6_CLKIN (STM32_PCLK1_FREQUENCY) #define STM32_APB1_TIM7_CLKIN (STM32_PCLK1_FREQUENCY) +/* MCO output */ + +#if defined(CONFIG_STM32_MII_MCO) || defined(CONFIG_STM32_RMII_MCO) +# define STM32_PLL_PLL3MUL RCC_CFGR2_PLL3MULx10 +#endif + /************************************************************************************ * Public Function Prototypes ************************************************************************************/ @@ -102,16 +117,3 @@ ************************************************************************************/ void stm32_boardinitialize(void); - -/************************************************************************************ - * Name: stm32_board_clockconfig - * - * Description: - * Any STM32 board may replace the "standard" board clock configuration logic with - * its own, custom clock cofiguration logic. - * - ************************************************************************************/ - -#ifdef CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG -void stm32_board_clockconfig(void); -#endif diff --git a/nuttx/configs/olimex-stm32-p107/nsh/defconfig b/nuttx/configs/olimex-stm32-p107/nsh/defconfig index 5c38a49efa..e3fb69ca2e 100644 --- a/nuttx/configs/olimex-stm32-p107/nsh/defconfig +++ b/nuttx/configs/olimex-stm32-p107/nsh/defconfig @@ -41,7 +41,6 @@ CONFIG_ARCH_CORTEXM3=y CONFIG_ARCH_CHIP="stm32" CONFIG_ARCH_CHIP_STM32F107VC=y CONFIG_ARCH_BOARD="olimex-stm32-p107" -CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG=y CONFIG_BOARD_LOOPSPERMSEC=5483 CONFIG_DRAM_START=0x20000000 CONFIG_DRAM_SIZE=65536 diff --git a/nuttx/configs/olimex-stm32-p107/ostest/defconfig b/nuttx/configs/olimex-stm32-p107/ostest/defconfig index 3710ee95b7..36fade11bb 100644 --- a/nuttx/configs/olimex-stm32-p107/ostest/defconfig +++ b/nuttx/configs/olimex-stm32-p107/ostest/defconfig @@ -41,7 +41,6 @@ CONFIG_ARCH_CORTEXM3=y CONFIG_ARCH_CHIP="stm32" CONFIG_ARCH_CHIP_STM32F107VC=y CONFIG_ARCH_BOARD="olimex-stm32-p107" -CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG=y CONFIG_BOARD_LOOPSPERMSEC=5483 CONFIG_DRAM_START=0x20000000 CONFIG_DRAM_SIZE=65536 diff --git a/nuttx/configs/olimex-stm32-p107/src/up_boot.c b/nuttx/configs/olimex-stm32-p107/src/up_boot.c index d8cba509e4..d7ece5a1e0 100644 --- a/nuttx/configs/olimex-stm32-p107/src/up_boot.c +++ b/nuttx/configs/olimex-stm32-p107/src/up_boot.c @@ -71,111 +71,3 @@ void stm32_boardinitialize(void) { } - -/************************************************************************************ - * Name: stm32_board_clockconfig - * - * Description: - * Any STM32 board may replace the "standard" board clock configuration logic with - * its own, custom clock cofiguration logic. - * - ************************************************************************************/ - -#ifdef CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG -void stm32_board_clockconfig(void) -{ - uint32_t regval; - - regval = getreg32(STM32_RCC_CR); - regval &= ~RCC_CR_HSEBYP; /* Disable HSE clock bypass */ - regval |= RCC_CR_HSEON; /* Enable HSE */ - putreg32(regval, STM32_RCC_CR); - - /* Set flash wait states - * Sysclk runs with 72MHz -> 2 waitstates. - * 0WS from 0-24MHz - * 1WS from 24-48MHz - * 2WS from 48-72MHz - */ - - regval = getreg32(STM32_FLASH_ACR); - regval &= ~FLASH_ACR_LATENCY_MASK; - regval |= (FLASH_ACR_LATENCY_2|FLASH_ACR_PRTFBE); - putreg32(regval, STM32_FLASH_ACR); - - regval = getreg32(STM32_RCC_CFGR2); - regval &= ~(RCC_CFGR2_PREDIV2_MASK - | RCC_CFGR2_PLL2MUL_MASK - | RCC_CFGR2_PREDIV1SRC_MASK - | RCC_CFGR2_PREDIV1_MASK); - regval |= RCC_CFGR2_PREDIV2d5; /* 25MHz / 5 */ - regval |= RCC_CFGR2_PLL2MULx8; /* 5MHz * 8 => 40MHz */ - regval |= RCC_CFGR2_PREDIV1SRC_PLL2; /* Use PLL2 as input for PREDIV1 */ - regval |= RCC_CFGR2_PREDIV1d5; /* 40MHz / 5 => 8MHz */ - putreg32(regval, STM32_RCC_CFGR2); - - /* Set the PCLK2 divider */ - - regval = getreg32(STM32_RCC_CFGR); - regval &= ~(RCC_CFGR_PPRE2_MASK | RCC_CFGR_HPRE_MASK); - regval |= STM32_RCC_CFGR_PPRE2; - regval |= RCC_CFGR_HPRE_SYSCLK; - putreg32(regval, STM32_RCC_CFGR); - - /* Set the PCLK1 divider */ - - regval = getreg32(STM32_RCC_CFGR); - regval &= ~RCC_CFGR_PPRE1_MASK; - regval |= STM32_RCC_CFGR_PPRE1; - putreg32(regval, STM32_RCC_CFGR); - - regval = getreg32(STM32_RCC_CR); - regval |= RCC_CR_PLL2ON; - putreg32(regval, STM32_RCC_CR); - - /* Wait for PLL2 ready */ - - while((getreg32(STM32_RCC_CR) & RCC_CR_PLL2RDY) == 0); - - /* Setup PLL3 for RMII clock on MCO */ - - regval = getreg32(STM32_RCC_CFGR2); - regval &= ~(RCC_CFGR2_PLL3MUL_MASK); - regval |= RCC_CFGR2_PLL3MULx10; - putreg32(regval, STM32_RCC_CFGR2); - - /* Switch PLL3 on */ - - regval = getreg32(STM32_RCC_CR); - regval |= RCC_CR_PLL3ON; - putreg32(regval, STM32_RCC_CR); - - while ((getreg32(STM32_RCC_CR) & RCC_CR_PLL3RDY) == 0); - - /* Set main PLL source 8MHz * 9 => 72MHz*/ - - regval = getreg32(STM32_RCC_CFGR); - regval &= ~(RCC_CFGR_PLLSRC | RCC_CFGR_PLLMUL_MASK); - regval |= (RCC_CFGR_PLLSRC | RCC_CFGR_PLLMUL_CLKx9); - putreg32(regval, STM32_RCC_CFGR); - - /* Switch main PLL on */ - - regval = getreg32(STM32_RCC_CR); - regval |= RCC_CR_PLLON; - putreg32(regval, STM32_RCC_CR); - - while ((getreg32(STM32_RCC_CR) & RCC_CR_PLLRDY) == 0); - - /* Select PLL as system clock source */ - - regval = getreg32(STM32_RCC_CFGR); - regval &= ~RCC_CFGR_SW_MASK; - regval |= RCC_CFGR_SW_PLL; - putreg32(regval, STM32_RCC_CFGR); - - /* Wait until PLL is used as the system clock source */ - - while ((getreg32(STM32_RCC_CFGR) & RCC_CFGR_SWS_PLL) == 0); -} -#endif diff --git a/nuttx/configs/shenzhou/include/board.h b/nuttx/configs/shenzhou/include/board.h index 2e82742ec6..8e4d895318 100644 --- a/nuttx/configs/shenzhou/include/board.h +++ b/nuttx/configs/shenzhou/include/board.h @@ -57,9 +57,18 @@ /* On-board crystal frequency is 25MHz (HSE) */ #define STM32_BOARD_XTAL 25000000ul -#define STM32_PLL_FREQUENCY (72000000) -#define STM32_SYSCLK_FREQUENCY STM32_PLL_FREQUENCY +/* PLL ouput is 72MHz */ + +#define STM32_PLL_PREDIV2 RCC_CFGR2_PREDIV2d5 /* 25MHz / 5 => 5MHz */ +#define STM32_PLL_PLL2MUL RCC_CFGR2_PLL2MULx8 /* 5MHz * 8 => 40MHz */ +#define STM32_PLL_PREDIV1 RCC_CFGR2_PREDIV1d5 /* 40MHz / 5 => 8MHz */ +#define STM32_PLL_PLLMUL RCC_CFGR_PLLMUL_CLKx9 /* 8MHz * 9 => 72Mhz */ +#define STM32_PLL_FREQUENCY (72000000) + +/* SYCLLK and HCLK are the PLL frequency */ + +#define STM32_SYSCLK_FREQUENCY STM32_PLL_FREQUENCY #define STM32_HCLK_FREQUENCY STM32_PLL_FREQUENCY #define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */ @@ -88,6 +97,12 @@ #define STM32_APB1_TIM6_CLKIN (STM32_PCLK1_FREQUENCY) #define STM32_APB1_TIM7_CLKIN (STM32_PCLK1_FREQUENCY) +/* MCO output */ + +#if defined(CONFIG_STM32_MII_MCO) || defined(CONFIG_STM32_RMII_MCO) +# define STM32_PLL_PLL3MUL RCC_CFGR2_PLL3MULx10 +#endif + /* LED definitions ******************************************************************/ /* If CONFIG_ARCH_LEDS is not defined, then the user can control the LEDs in any * way. The following definitions are used to access individual LEDs. @@ -316,19 +331,6 @@ void stm32_boardinitialize(void); -/************************************************************************************ - * Name: stm32_board_clockconfig - * - * Description: - * Any STM32 board may replace the "standard" board clock configuration logic with - * its own, custom clock cofiguration logic. - * - ************************************************************************************/ - -#ifdef CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG -void stm32_board_clockconfig(void); -#endif - /************************************************************************************ * Button support. * diff --git a/nuttx/configs/shenzhou/nsh/defconfig b/nuttx/configs/shenzhou/nsh/defconfig index 70d9540db8..f231b26325 100644 --- a/nuttx/configs/shenzhou/nsh/defconfig +++ b/nuttx/configs/shenzhou/nsh/defconfig @@ -161,7 +161,7 @@ CONFIG_STM32_JTAG_FULL_ENABLE=y # CONFIG_STM32_JTAG_NOJNTRST_ENABLE is not set # CONFIG_STM32_JTAG_SW_ENABLE is not set # CONFIG_STM32_FORCEPOWER is not set -CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG=y +# CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG is not set` # # SPI Configuration diff --git a/nuttx/configs/shenzhou/src/Makefile b/nuttx/configs/shenzhou/src/Makefile index 50ddaa5cad..c12a251f0f 100644 --- a/nuttx/configs/shenzhou/src/Makefile +++ b/nuttx/configs/shenzhou/src/Makefile @@ -42,10 +42,6 @@ AOBJS = $(ASRCS:.S=$(OBJEXT)) CSRCS = up_boot.c up_spi.c up_mmcsd.c -ifeq ($(CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG),y) -CSRCS += up_clockconfig.c -endif - ifeq ($(CONFIG_HAVE_CXX),y) CSRCS += up_cxxinitialize.c endif diff --git a/nuttx/configs/shenzhou/src/up_clockconfig.c b/nuttx/configs/shenzhou/src/up_clockconfig.c deleted file mode 100644 index 6c3bd56e14..0000000000 --- a/nuttx/configs/shenzhou/src/up_clockconfig.c +++ /dev/null @@ -1,167 +0,0 @@ -/************************************************************************************ - * configs/olimex-stm32-p107/src/up_boot.c - * arch/arm/src/board/up_boot.c - * - * Copyright (C) 2009, 2012 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 "up_arch.h" - -/************************************************************************************ - * Pre-processor Definitions - ************************************************************************************/ - -/************************************************************************************ - * Private Functions - ************************************************************************************/ - -/************************************************************************************ - * Public Functions - ************************************************************************************/ - -/************************************************************************************ - * Name: stm32_board_clockconfig - * - * Description: - * Any STM32 board may replace the "standard" board clock configuration logic with - * its own, custom clock cofiguration logic. - * - ************************************************************************************/ - -#ifdef CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG -void stm32_board_clockconfig(void) -{ - uint32_t regval; - - regval = getreg32(STM32_RCC_CR); - regval &= ~RCC_CR_HSEBYP; /* Disable HSE clock bypass */ - regval |= RCC_CR_HSEON; /* Enable HSE */ - putreg32(regval, STM32_RCC_CR); - - /* Set flash wait states - * Sysclk runs with 72MHz -> 2 waitstates. - * 0WS from 0-24MHz - * 1WS from 24-48MHz - * 2WS from 48-72MHz - */ - - regval = getreg32(STM32_FLASH_ACR); - regval &= ~FLASH_ACR_LATENCY_MASK; - regval |= (FLASH_ACR_LATENCY_2|FLASH_ACR_PRTFBE); - putreg32(regval, STM32_FLASH_ACR); - - regval = getreg32(STM32_RCC_CFGR2); - regval &= ~(RCC_CFGR2_PREDIV2_MASK - | RCC_CFGR2_PLL2MUL_MASK - | RCC_CFGR2_PREDIV1SRC_MASK - | RCC_CFGR2_PREDIV1_MASK); - regval |= RCC_CFGR2_PREDIV2d5; /* 25MHz / 5 */ - regval |= RCC_CFGR2_PLL2MULx8; /* 5MHz * 8 => 40MHz */ - regval |= RCC_CFGR2_PREDIV1SRC_PLL2; /* Use PLL2 as input for PREDIV1 */ - regval |= RCC_CFGR2_PREDIV1d5; /* 40MHz / 5 => 8MHz */ - putreg32(regval, STM32_RCC_CFGR2); - - /* Set the PCLK2 divider */ - - regval = getreg32(STM32_RCC_CFGR); - regval &= ~(RCC_CFGR_PPRE2_MASK | RCC_CFGR_HPRE_MASK); - regval |= STM32_RCC_CFGR_PPRE2; - regval |= RCC_CFGR_HPRE_SYSCLK; - putreg32(regval, STM32_RCC_CFGR); - - /* Set the PCLK1 divider */ - - regval = getreg32(STM32_RCC_CFGR); - regval &= ~RCC_CFGR_PPRE1_MASK; - regval |= STM32_RCC_CFGR_PPRE1; - putreg32(regval, STM32_RCC_CFGR); - - regval = getreg32(STM32_RCC_CR); - regval |= RCC_CR_PLL2ON; - putreg32(regval, STM32_RCC_CR); - - /* Wait for PLL2 ready */ - - while((getreg32(STM32_RCC_CR) & RCC_CR_PLL2RDY) == 0); - - /* Setup PLL3 for RMII clock on MCO */ - - regval = getreg32(STM32_RCC_CFGR2); - regval &= ~(RCC_CFGR2_PLL3MUL_MASK); - regval |= RCC_CFGR2_PLL3MULx10; - putreg32(regval, STM32_RCC_CFGR2); - - /* Switch PLL3 on */ - - regval = getreg32(STM32_RCC_CR); - regval |= RCC_CR_PLL3ON; - putreg32(regval, STM32_RCC_CR); - - while ((getreg32(STM32_RCC_CR) & RCC_CR_PLL3RDY) == 0); - - /* Set main PLL source 8MHz * 9 => 72MHz*/ - - regval = getreg32(STM32_RCC_CFGR); - regval &= ~(RCC_CFGR_PLLSRC | RCC_CFGR_PLLMUL_MASK); - regval |= (RCC_CFGR_PLLSRC | RCC_CFGR_PLLMUL_CLKx9); - putreg32(regval, STM32_RCC_CFGR); - - /* Switch main PLL on */ - - regval = getreg32(STM32_RCC_CR); - regval |= RCC_CR_PLLON; - putreg32(regval, STM32_RCC_CR); - - while ((getreg32(STM32_RCC_CR) & RCC_CR_PLLRDY) == 0); - - /* Select PLL as system clock source */ - - regval = getreg32(STM32_RCC_CFGR); - regval &= ~RCC_CFGR_SW_MASK; - regval |= RCC_CFGR_SW_PLL; - putreg32(regval, STM32_RCC_CFGR); - - /* Wait until PLL is used as the system clock source */ - - while ((getreg32(STM32_RCC_CFGR) & RCC_CFGR_SWS_PLL) == 0); -} -#endif From a9b21886f32d0a8ef7cad8dfe7efbc3276f4fd58 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Sep 2012 18:44:01 +0200 Subject: [PATCH 18/56] Halfway-working fixed wing waypoint control, needs more effort --- apps/fixedwing_control/fixedwing_control.c | 1068 +++++------------ .../multirotor_att_control_main.c | 6 +- .../multirotor_pos_control/position_control.c | 40 - apps/systemlib/Makefile | 3 +- apps/systemlib/geo/geo.c | 88 ++ apps/systemlib/geo/geo.h | 49 + 6 files changed, 439 insertions(+), 815 deletions(-) create mode 100644 apps/systemlib/geo/geo.c create mode 100644 apps/systemlib/geo/geo.h diff --git a/apps/fixedwing_control/fixedwing_control.c b/apps/fixedwing_control/fixedwing_control.c index 67e5d1b289..1323f2566d 100644 --- a/apps/fixedwing_control/fixedwing_control.c +++ b/apps/fixedwing_control/fixedwing_control.c @@ -44,16 +44,12 @@ #include #include #include -#include #include -#include +#include #include #include #include -#include -#include #include -#include #include #include #include @@ -62,6 +58,8 @@ #include #include #include +#include +#include static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ @@ -82,6 +80,278 @@ int fixedwing_control_thread_main(int argc, char *argv[]); */ static void usage(const char *reason); +/* + * Controller parameters, accessible via MAVLink + * + */ +PARAM_DEFINE_FLOAT(FW_ROLL_POS_P, 0.3f); +PARAM_DEFINE_FLOAT(FW_ROLL_POS_I, 0.002f); +PARAM_DEFINE_FLOAT(FW_ROLL_POS_AWU, 0.2f); +PARAM_DEFINE_FLOAT(FW_ROLL_POS_LIM, 0.6f); + +struct fw_att_control_params { + float roll_pos_p; + float roll_pos_i; + float roll_pos_awu; + float roll_pos_lim; +}; + +struct fw_att_control_param_handles { + param_t roll_pos_p; + param_t roll_pos_i; + param_t roll_pos_awu; + param_t roll_pos_lim; +}; + + +// XXX outsource position control to a separate app some time + +PARAM_DEFINE_FLOAT(FW_HEADING_P, 0.1f); +PARAM_DEFINE_FLOAT(FW_HEADING_LIM, 0.15f); + +struct fw_pos_control_params { + float heading_p; + float heading_lim; +}; + +struct fw_pos_control_param_handles { + param_t heading_p; + param_t heading_lim; +}; + +/** + * Initialize all parameter handles and values + * + */ +static int att_parameters_init(struct fw_att_control_param_handles *h); + +/** + * Update all parameters + * + */ +static int att_parameters_update(const struct fw_att_control_param_handles *h, struct fw_att_control_params *p); + +/** + * Initialize all parameter handles and values + * + */ +static int pos_parameters_init(struct fw_pos_control_param_handles *h); + +/** + * Update all parameters + * + */ +static int pos_parameters_update(const struct fw_pos_control_param_handles *h, struct fw_pos_control_params *p); + + +/** + * The fixed wing control main loop. + * + * This loop executes continously and calculates the control + * response. + * + * @param argc number of arguments + * @param argv argument array + * + * @return 0 + * + */ +int fixedwing_control_thread_main(int argc, char *argv[]) +{ + // /* read arguments */ + // bool verbose = false; + + // for (int i = 1; i < argc; i++) { + // if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) { + // verbose = true; + // } + // } + + /* welcome user */ + printf("[fixedwing control] started\n"); + + /* output structs */ + struct actuator_controls_s actuators; + struct vehicle_attitude_setpoint_s att_sp; + memset(&att_sp, 0, sizeof(att_sp)); + + /* publish actuator controls */ + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) + actuators.control[i] = 0.0f; + orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); + orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); + + /* Subscribe to global position, attitude and rc */ + /* declare and safely initialize all structs */ + struct vehicle_status_s state; + memset(&state, 0, sizeof(state)); + struct vehicle_attitude_s att; + memset(&att_sp, 0, sizeof(att_sp)); + struct manual_control_setpoint_s manual; + memset(&manual, 0, sizeof(manual)); + + /* subscribe to attitude, motor setpoints and system state */ + struct vehicle_global_position_s global_pos; + int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + struct vehicle_global_position_setpoint_s global_setpoint; + int global_setpoint_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); + int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + int state_sub = orb_subscribe(ORB_ID(vehicle_status)); + int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + + /* Mainloop setup */ + unsigned int loopcounter = 0; + + uint64_t last_run = 0; + uint64_t last_run_pos = 0; + + bool global_sp_updated_set_once = false; + + struct fw_att_control_params p; + struct fw_att_control_param_handles h; + + struct fw_pos_control_params ppos; + struct fw_pos_control_param_handles hpos; + + /* initialize the pid controllers */ + att_parameters_init(&h); + att_parameters_update(&h, &p); + + pos_parameters_init(&hpos); + pos_parameters_update(&hpos, &ppos); + + PID_t roll_pos_controller; + pid_init(&roll_pos_controller, p.roll_pos_p, p.roll_pos_i, 0.0f, p.roll_pos_awu, + PID_MODE_DERIVATIV_SET, 155); + + PID_t heading_controller; + pid_init(&heading_controller, ppos.heading_p, 0.0f, 0.0f, 0.0f, + PID_MODE_DERIVATIV_SET, 155); + + while(!thread_should_exit) { + + struct pollfd fds[1] = { + { .fd = att_sub, .events = POLLIN }, + }; + int ret = poll(fds, 1, 1000); + + if (ret < 0) { + /* XXX this is seriously bad - should be an emergency */ + } else if (ret == 0) { + /* XXX this means no sensor data - should be critical or emergency */ + printf("[fixedwing control] WARNING: Not getting attitude - estimator running?\n"); + } else { + + // FIXME SUBSCRIBE + if (loopcounter % 20 == 0) { + pid_set_parameters(&roll_pos_controller, p.roll_pos_p, p.roll_pos_i, 0.0f, p.roll_pos_awu); + pid_set_parameters(&heading_controller, ppos.heading_p, 0.0f, 0.0f, 0.0f); + } + + /* if position updated, run position control loop */ + bool pos_updated; + orb_check(global_pos_sub, &pos_updated); + bool global_sp_updated; + orb_check(global_setpoint_sub, &global_sp_updated); + if (global_sp_updated) { + global_sp_updated_set_once = true; + } + /* checking has to happen before the read, as the read clears the changed flag */ + + /* get a local copy of system state */ + orb_copy(ORB_ID(vehicle_status), state_sub, &state); + /* get a local copy of manual setpoint */ + orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); + /* get a local copy of attitude */ + orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); + /* get a local copy of attitude setpoint */ + orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp); + + if (pos_updated) { + + /* get position */ + orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos); + + if (global_sp_updated_set_once) { + orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint); + + + /* calculate delta T */ + const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; + last_run = hrt_absolute_time(); + + /* calculate bearing error */ + float target_bearing = get_bearing_to_next_waypoint(global_pos.lat / (double)1e7d, global_pos.lon / (double)1e7d, + global_setpoint.lat / (double)1e7d, global_setpoint.lon / (double)1e7d); + + /* shift error to prevent wrapping issues */ + float bearing_error = att.yaw - target_bearing; + + if (bearing_error < M_PI_F) { + bearing_error += 2.0f * M_PI_F; + } + + if (bearing_error > M_PI_F) { + bearing_error -= 2.0f * M_PI_F; + } + + /* calculate roll setpoint, do this artificially around zero */ + att_sp.roll_body = pid_calculate(&heading_controller, bearing_error, + 0.0f, att.yawspeed, deltaT); + + /* limit roll angle output */ + if (att_sp.roll_body > ppos.heading_lim) { + att_sp.roll_body = ppos.heading_lim; + heading_controller.saturated = 1; + } + + if (att_sp.roll_body < -ppos.heading_lim) { + att_sp.roll_body = -ppos.heading_lim; + heading_controller.saturated = 1; + } + + att_sp.roll_body = att_sp.roll_body; + att_sp.pitch_body = 0.0f; + att_sp.yaw_body = 0.0f; + + } else { + /* no setpoint, maintain level flight */ + att_sp.roll_body = 0.0f; + att_sp.pitch_body = 0.0f; + att_sp.yaw_body = 0.0f; + } + + att_sp.thrust = 0.7f; + } + + /* calculate delta T */ + const float deltaTpos = (hrt_absolute_time() - last_run_pos) / 1000000.0f; + last_run_pos = hrt_absolute_time(); + + actuators.control[0] = pid_calculate(&roll_pos_controller, att_sp.roll_body, + att.roll, att.rollspeed, deltaTpos); + actuators.control[1] = 0; + actuators.control[2] = 0; + actuators.control[3] = att_sp.thrust; + + /* publish attitude setpoint (for MAVLink) */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + + /* publish actuator setpoints (for mixer) */ + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + + loopcounter++; + + } + } + + printf("[fixedwing_control] exiting.\n"); + thread_running = false; + + return 0; +} + static void usage(const char *reason) { @@ -136,783 +406,37 @@ int fixedwing_control_main(int argc, char *argv[]) exit(1); } -#define PID_DT 0.01f -#define PID_SCALER 0.1f -#define PID_DERIVMODE_CALC 0 -#define HIL_MODE 32 -#define AUTO -1000 -#define MANUAL 3000 -#define SERVO_MIN 1000 -#define SERVO_MAX 2000 - -pthread_t control_thread; -pthread_t nav_thread; -pthread_t servo_thread; - -/** - * Servo channels function enumerator used for - * the servo writing part - */ -enum SERVO_CHANNELS_FUNCTION { - - AIL_1 = 0, - AIL_2 = 1, - MOT = 2, - ACT_1 = 3, - ACT_2 = 4, - ACT_3 = 5, - ACT_4 = 6, - ACT_5 = 7 -}; - -/** - * The plane_data structure. - * - * The plane data structure is the local storage of all the flight information of the aircraft - */ -typedef struct { - double lat; - double lon; - float alt; - float vx; - float vy; - float vz; - float yaw; - float hdg; - float pitch; - float roll; - float yawspeed; - float pitchspeed; - float rollspeed; - float rollb; /* body frame angles */ - float pitchb; - float yawb; - float p; - float q; - float r; /* body angular rates */ - - /* PID parameters*/ - - float Kp_att; - float Ki_att; - float Kd_att; - float Kp_pos; - float Ki_pos; - float Kd_pos; - float intmax_att; - float intmax_pos; - - /* Next waypoint*/ - - double wp_x; - double wp_y; - float wp_z; - - /* Setpoints */ - - float airspeed; - float groundspeed; - float roll_setpoint; - float pitch_setpoint; - float throttle_setpoint; - - /* Navigation mode*/ - int mode; - -} plane_data_t; - -/** - * The control_outputs structure. - * - * The control outputs structure contains the control outputs - * of the aircraft - */ -typedef struct { - float roll_ailerons; - float pitch_elevator; - float yaw_rudder; - float throttle; - // set the aux values to 0 per default - float aux1; - float aux2; - float aux3; - float aux4; - uint8_t mode; // HIL_ENABLED: 32 - uint8_t nav_mode; -} control_outputs_t; - -/** - * Generic PID algorithm with PD scaling - */ -static float pid(float error, float error_deriv, uint16_t dt, float scaler, float K_p, float K_i, float K_d, float intmax); - -/* - * Output calculations - */ - -static void calc_body_angular_rates(float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed); -static void calc_rotation_matrix(float roll, float pitch, float yaw, float x, float y, float z); -static void calc_bodyframe_angles(float roll, float pitch, float yaw); -static float calc_bearing(void); -static float calc_roll_ail(void); -static float calc_pitch_elev(void); -static float calc_yaw_rudder(float hdg); -static float calc_throttle(void); -static float calc_gnd_speed(void); -static void get_parameters(plane_data_t *plane_data); -static float calc_roll_setpoint(void); -static float calc_pitch_setpoint(void); -static float calc_throttle_setpoint(void); -static float calc_wp_distance(void); -static void set_plane_mode(void); - -/**************************************************************************** - * Public Data - ****************************************************************************/ - -plane_data_t plane_data; -control_outputs_t control_outputs; -float scaler = 1; //M_PI; - -/**************************************************************************** - * Private Functions - ****************************************************************************/ - -/** - * Calculates the PID control output given an error. - * - * Incorporates PD scaling and low-pass filter for the derivative component. - * - * @param error the input error - * @param error_deriv the derivative of the input error - * @param dt time constant - * @param scaler PD scaler - * @param K_p P gain - * @param K_i I gain - * @param K_d D gain - * @param intmax Integration limit - * - * @return the PID control output - */ - -static float pid(float error, float error_deriv, uint16_t dt, float scale, float K_p, float K_i, float K_d, float intmax) +static int att_parameters_init(struct fw_att_control_param_handles *h) { - float Kp = K_p; - float Ki = K_i; - float Kd = K_d; - float delta_time = dt; - float lerror; - float imax = intmax; - float integrator; - float derivative; - float lderiv; - int fCut = 20; /* anything above 20 Hz is considered noise - low pass filter for the derivative */ - float output = 0; + /* PID parameters */ + h->roll_pos_p = param_find("FW_ROLL_POS_P"); + h->roll_pos_i = param_find("FW_ROLL_POS_I"); + h->roll_pos_lim = param_find("FW_ROLL_POS_LIM"); + h->roll_pos_awu = param_find("FW_ROLL_POS_AWU"); - output += error * Kp; - - if ((fabsf(Kd) > 0) && (dt > 0)) { - - if (PID_DERIVMODE_CALC) { - derivative = (error - lerror) / delta_time; - - /* - * discrete low pass filter, cuts out the - * high frequency noise that can drive the controller crazy - */ - float RC = 1.0f / (2.0f * M_PI_F * fCut); - derivative = lderiv + - (delta_time / (RC + delta_time)) * (derivative - lderiv); - - /* update state */ - lerror = error; - lderiv = derivative; - - } else { - derivative = error_deriv; - } - - /* add in derivative component */ - output += Kd * derivative; - } - - //printf("PID derivative %i\n", (int)(1000*derivative)); - - /* scale the P and D components with the PD scaler */ - output *= scale; - - /* Compute integral component if time has elapsed */ - if ((fabsf(Ki) > 0) && (dt > 0)) { - integrator += (error * Ki) * scaler * delta_time; - - if (integrator < -imax) { - integrator = -imax; - - } else if (integrator > imax) { - integrator = imax; - } - - output += integrator; - } - - //printf("PID Integrator %i\n", (int)(1000*integrator)); - - return output; + return OK; } -PARAM_DEFINE_FLOAT(FW_ATT_P, 2.0f); -PARAM_DEFINE_FLOAT(FW_ATT_I, 0.0f); -PARAM_DEFINE_FLOAT(FW_ATT_D, 0.0f); -PARAM_DEFINE_FLOAT(FW_ATT_AWU, 5.0f); -PARAM_DEFINE_FLOAT(FW_ATT_LIM, 10.0f); - -PARAM_DEFINE_FLOAT(FW_POS_P, 2.0f); -PARAM_DEFINE_FLOAT(FW_POS_I, 0.0f); -PARAM_DEFINE_FLOAT(FW_POS_D, 0.0f); -PARAM_DEFINE_FLOAT(FW_POS_AWU, 5.0f); -PARAM_DEFINE_FLOAT(FW_POS_LIM, 10.0f); - -/** - * Load parameters from global storage. - * - * @param plane_data Fixed wing data structure - * - * Fetches the current parameters from the global parameter storage and writes them - * to the plane_data structure - */ -static void get_parameters(plane_data_t * pdata) +static int att_parameters_update(const struct fw_att_control_param_handles *h, struct fw_att_control_params *p) { - static bool initialized = false; - static param_t att_p; - static param_t att_i; - static param_t att_d; - static param_t att_awu; - static param_t att_lim; + param_get(h->roll_pos_p, &(p->roll_pos_p)); - static param_t pos_p; - static param_t pos_i; - static param_t pos_d; - static param_t pos_awu; - static param_t pos_lim; - - if (!initialized) { - att_p = param_find("FW_ATT_P"); - att_i = param_find("FW_ATT_I"); - att_d = param_find("FW_ATT_D"); - att_awu = param_find("FW_ATT_AWU"); - att_lim = param_find("FW_ATT_LIM"); - - pos_p = param_find("FW_POS_P"); - pos_i = param_find("FW_POS_I"); - pos_d = param_find("FW_POS_D"); - pos_awu = param_find("FW_POS_AWU"); - pos_lim = param_find("FW_POS_LIM"); - - initialized = true; - } - - param_get(att_p, &(pdata->Kp_att)); - param_get(att_i, &(pdata->Ki_att)); - param_get(att_d, &(pdata->Kd_att)); - param_get(pos_p, &(pdata->Kp_pos)); - param_get(pos_i, &(pdata->Ki_pos)); - param_get(pos_d, &(pdata->Kd_pos)); - param_get(att_awu, &(pdata->intmax_att)); - param_get(pos_awu, &(pdata->intmax_pos)); - pdata->airspeed = 10; - pdata->mode = 1; + return OK; } -/** - * Calculates the body angular rates. - * - * Calculates the rates of the plane using inertia matrix and - * writes them to the plane_data structure - * - * @param roll - * @param pitch - * @param yaw - * @param rollspeed - * @param pitchspeed - * @param yawspeed - * - */ -static void calc_body_angular_rates(float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed) +static int pos_parameters_init(struct fw_pos_control_param_handles *h) { - plane_data.p = rollspeed - sinf(pitch) * yawspeed; - plane_data.q = cosf(roll) * pitchspeed + sinf(roll) * cosf(pitch) * yawspeed; - plane_data.r = -sinf(roll) * pitchspeed + cosf(roll) * cosf(pitch) * yawspeed; + /* PID parameters */ + h->heading_p = param_find("FW_HEADING_P"); + h->heading_lim = param_find("FW_HEADING_LIM"); + + return OK; } -/** - * - * Calculates the attitude angles in the body reference frame. - * - * Writes them to the plane data structure - * - * @param roll - * @param pitch - * @param yaw - */ - -static void calc_bodyframe_angles(float roll, float pitch, float yaw) +static int pos_parameters_update(const struct fw_pos_control_param_handles *h, struct fw_pos_control_params *p) { - plane_data.rollb = cosf(yaw) * cosf(pitch) * roll + - (cosf(yaw) * sinf(pitch) * sinf(roll) + sinf(yaw) * cosf(roll)) * pitch - + (-cosf(yaw) * sinf(pitch) * cosf(roll) + sinf(yaw) * sinf(roll)) * yaw; - plane_data.pitchb = -sinf(yaw) * cosf(pitch) * roll + - (-sinf(yaw) * sinf(pitch) * sinf(roll) + cosf(yaw) * cosf(roll)) * pitch - + (sinf(yaw) * sinf(pitch) * cosf(roll) + cosf(yaw) * sinf(roll)) * yaw; - plane_data.yawb = sinf(pitch) * roll - cosf(pitch) * sinf(roll) * pitch + cosf(pitch) * cosf(roll) * yaw; -} - -/** - * calc_rotation_matrix - * - * Calculates the rotation matrix - * - * @param roll - * @param pitch - * @param yaw - * @param x - * @param y - * @param z - * - */ - -static void calc_rotation_matrix(float roll, float pitch, float yaw, float x, float y, float z) -{ - plane_data.rollb = cosf(yaw) * cosf(pitch) * x + - (cosf(yaw) * sinf(pitch) * sinf(roll) + sinf(yaw) * cosf(roll)) * y - + (-cosf(yaw) * sinf(pitch) * cosf(roll) + sinf(yaw) * sinf(roll)) * z; - plane_data.pitchb = -sinf(yaw) * cosf(pitch) * x + - (-sinf(yaw) * sinf(pitch) * sinf(roll) + cosf(yaw) * cosf(roll)) * y - + (sinf(yaw) * sinf(pitch) * cosf(roll) + cosf(yaw) * sinf(roll)) * z; - plane_data.yawb = sinf(pitch) * x - cosf(pitch) * sinf(roll) * y + cosf(pitch) * cosf(roll) * z; -} - -/** - * calc_bearing - * - * Calculates the bearing error of the plane compared to the waypoints - * - * @return bearing Bearing error - * - */ -static float calc_bearing() -{ - float bearing = M_PI_F/2.0f + (float)atan2(-(plane_data.wp_y - plane_data.lat), (plane_data.wp_x - plane_data.lon)); - - if (bearing < 0.0f) { - bearing += 2*M_PI_F; - } - - return bearing; -} - -/** - * calc_roll_ail - * - * Calculates the roll ailerons control output - * - * @return Roll ailerons control output (-1,1) - */ - -static float calc_roll_ail() -{ - float ret = pid((plane_data.roll_setpoint - plane_data.roll), plane_data.rollspeed, PID_DT, PID_SCALER, - plane_data.Kp_att, plane_data.Ki_att, plane_data.Kd_att, plane_data.intmax_att); - - if (ret < -1) - return -1; - - if (ret > 1) - return 1; - - return ret; -} - -/** - * calc_pitch_elev - * - * Calculates the pitch elevators control output - * - * @return Pitch elevators control output (-1,1) - */ -static float calc_pitch_elev() -{ - float ret = pid((plane_data.pitch_setpoint - plane_data.pitch), plane_data.pitchspeed, PID_DT, PID_SCALER, - plane_data.Kp_att, plane_data.Ki_att, plane_data.Kd_att, plane_data.intmax_att); - - if (ret < -1) - return -1; - - if (ret > 1) - return 1; - - return ret; -} - -/** - * calc_yaw_rudder - * - * Calculates the yaw rudder control output (only if yaw rudder exists on the model) - * - * @return Yaw rudder control output (-1,1) - * - */ -static float calc_yaw_rudder(float hdg) -{ - float ret = pid((plane_data.yaw - abs(hdg)), plane_data.yawspeed, PID_DT, PID_SCALER, - plane_data.Kp_pos, plane_data.Ki_pos, plane_data.Kd_pos, plane_data.intmax_pos); - - if (ret < -1) - return -1; - - if (ret > 1) - return 1; - - return ret; -} - -/** - * calc_throttle - * - * Calculates the throttle control output - * - * @return Throttle control output (0,1) - */ - -static float calc_throttle() -{ - float ret = pid(plane_data.throttle_setpoint - calc_gnd_speed(), 0, PID_DT, PID_SCALER, - plane_data.Kp_pos, plane_data.Ki_pos, plane_data.Kd_pos, plane_data.intmax_pos); - - if (ret < 0.2f) - return 0.2f; - - if (ret > 1.0f) - return 1.0f; - - return ret; -} - -/** - * calc_gnd_speed - * - * Calculates the ground speed using the x and y components - * - * Input: none (operation on global data) - * - * Output: Ground speed of the plane - * - */ - -static float calc_gnd_speed() -{ - float gnd_speed = sqrtf(plane_data.vx * plane_data.vx + plane_data.vy * plane_data.vy); - return gnd_speed; -} - -/** - * calc_wp_distance - * - * Calculates the distance to the next waypoint - * - * @return the distance to the next waypoint - * - */ - -static float calc_wp_distance() -{ - return sqrtf((plane_data.lat - plane_data.wp_y) * (plane_data.lat - plane_data.wp_y) + - (plane_data.lon - plane_data.wp_x) * (plane_data.lon - plane_data.wp_x)); -} - -/** - * calc_roll_setpoint - * - * Calculates the offset angle for the roll plane, - * saturates at +- 35 deg. - * - * @return setpoint on which attitude control should stabilize while changing heading - * - */ - -static float calc_roll_setpoint() -{ - float setpoint = 0; - - setpoint = calc_bearing() - plane_data.yaw; - - if (setpoint < (-35.0f/180.0f)*M_PI_F) - return (-35.0f/180.0f)*M_PI_F; - - if (setpoint > (35/180.0f)*M_PI_F) - return (-35.0f/180.0f)*M_PI_F; - - return setpoint; -} - -/** - * calc_pitch_setpoint - * - * Calculates the offset angle for the pitch plane - * saturates at +- 35 deg. - * - * @return setpoint on which attitude control should stabilize while changing altitude - * - */ - -static float calc_pitch_setpoint() -{ - float setpoint = 0.0f; - - // if (plane_data.mode == TAKEOFF) { - // setpoint = ((35/180.0f)*M_PI_F); - - // } else { - setpoint = atanf((plane_data.wp_z - plane_data.alt) / calc_wp_distance()); - - if (setpoint < (-35.0f/180.0f)*M_PI_F) - return (-35.0f/180.0f)*M_PI_F; - - if (setpoint > (35/180.0f)*M_PI_F) - return (-35.0f/180.0f)*M_PI_F; - // } - - return setpoint; -} - -/** - * - * Calculates the throttle setpoint for different flight modes - * - * @return throttle output setpoint - * - */ -static float calc_throttle_setpoint() -{ - float setpoint = 0; - - // // if TAKEOFF full throttle - // if (plane_data.mode == TAKEOFF) { - // setpoint = 60.0f; - // } - - // // if CRUISE - parameter value - // if (plane_data.mode == CRUISE) { - setpoint = plane_data.airspeed; - // } - - // // if LAND no throttle - // if (plane_data.mode == LAND) { - // setpoint = 0.0f; - // } - - return setpoint; -} - -/** - * - * @param argc number of arguments - * @param argv argument array - * - * @return 0 - * - */ -int fixedwing_control_thread_main(int argc, char *argv[]) -{ - /* default values for arguments */ - char *fixedwing_uart_name = "/dev/ttyS1"; - char *commandline_usage = "\tusage: %s -d fixedwing-devicename\n"; - - /* read arguments */ - bool verbose = false; - - for (int i = 1; i < argc; i++) { - if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { - if (argc > i + 1) { - fixedwing_uart_name = argv[i + 1]; - - } else { - printf(commandline_usage, argv[0]); - return 0; - } - } - if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) { - verbose = true; - } - } - - /* welcome user */ - printf("[fixedwing control] started\n"); - - /* output structs */ - struct actuator_controls_s actuators; - struct vehicle_attitude_setpoint_s att_sp; - memset(&att_sp, 0, sizeof(att_sp)); - - /* publish actuator controls */ - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) - actuators.control[i] = 0.0f; - orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); - orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); - - /* Subscribe to global position, attitude and rc */ - struct vehicle_global_position_s global_pos; - int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - struct vehicle_global_position_setpoint_s global_setpoint; - int global_setpoint_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); - /* declare and safely initialize all structs */ - struct vehicle_status_s state; - memset(&state, 0, sizeof(state)); - struct vehicle_attitude_s att; - memset(&att_sp, 0, sizeof(att_sp)); - struct manual_control_setpoint_s manual; - memset(&manual, 0, sizeof(manual)); - - /* subscribe to attitude, motor setpoints and system state */ - int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - int state_sub = orb_subscribe(ORB_ID(vehicle_status)); - int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - - /* Mainloop setup */ - unsigned int loopcounter = 0; - unsigned int failcounter = 0; - - /* Control constants */ - control_outputs.mode = HIL_MODE; - control_outputs.nav_mode = 0; - - /* Servo setup */ - - /* - * Main control, navigation and servo routine - */ - - while(1) { - /* - * DATA Handling - * Fetch current flight data - */ - - /* get position, attitude and rc inputs */ - // XXX add error checking - orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos); - orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint); - - /* get a local copy of system state */ - orb_copy(ORB_ID(vehicle_status), state_sub, &state); - /* get a local copy of manual setpoint */ - orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); - /* get a local copy of attitude */ - orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); - /* get a local copy of attitude setpoint */ - orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp); - - /* scaling factors are defined by the data from the APM Planner - * TODO: ifdef for other parameters (HIL/Real world switch) - */ - - /* position values*/ - plane_data.lat = global_pos.lat / 10000000.0; - plane_data.lon = global_pos.lon / 10000000.0; - plane_data.alt = global_pos.alt / 1000.0f; - plane_data.vx = global_pos.vx / 100.0f; - plane_data.vy = global_pos.vy / 100.0f; - plane_data.vz = global_pos.vz / 100.0f; - - /* attitude values*/ - plane_data.roll = att.roll; - plane_data.pitch = att.pitch; - plane_data.yaw = att.yaw; - plane_data.rollspeed = att.rollspeed; - plane_data.pitchspeed = att.pitchspeed; - plane_data.yawspeed = att.yawspeed; - - plane_data.wp_x = global_setpoint.lat / (double)1e7; - plane_data.wp_y = global_setpoint.lon / (double)1e7; - plane_data.wp_z = global_setpoint.altitude; - - /* parameter values */ - if (loopcounter % 20 == 0) { - get_parameters(&plane_data); - } - - /* Attitude control part */ - - if (verbose && loopcounter % 20 == 0) { - /******************************** DEBUG OUTPUT ************************************************************/ - - printf("Parameter: %i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i\n", (int)plane_data.Kp_att, (int)plane_data.Ki_att, - (int)plane_data.Kd_att, (int)plane_data.intmax_att, (int)plane_data.Kp_pos, (int)plane_data.Ki_pos, - (int)plane_data.Kd_pos, (int)plane_data.intmax_pos, (int)plane_data.airspeed, - (int)plane_data.wp_x, (int)plane_data.wp_y, (int)plane_data.wp_z); - - printf("PITCH SETPOINT: %i\n", (int)(100.0f*plane_data.pitch_setpoint)); - printf("ROLL SETPOINT: %i\n", (int)(100.0f*plane_data.roll_setpoint)); - printf("THROTTLE SETPOINT: %i\n", (int)(100.0f*calc_throttle_setpoint())); - - // printf("\n\nVx: %i\t Vy: %i\t Current speed:%i\n\n", (int)plane_data.vx, (int)plane_data.vy, (int)(calc_gnd_speed())); - - // printf("Current Altitude: %i\n\n", (int)plane_data.alt); - - printf("\nAttitude values: \n R:%i \n P: %i \n Y: %i \n\n RS: %i \n PS: %i \n YS: %i \n ", - (int)(180.0f * plane_data.roll/M_PI_F), (int)(180.0f * plane_data.pitch/M_PI_F), (int)(180.0f * plane_data.yaw/M_PI_F), - (int)(180.0f * plane_data.rollspeed/M_PI_F), (int)(180.0f * plane_data.pitchspeed/M_PI_F), (int)(180.0f * plane_data.yawspeed)/M_PI_F); - - // printf("\nBody Rates: \n P: %i \n Q: %i \n R: %i \n", - // (int)(10000 * plane_data.p), (int)(10000 * plane_data.q), (int)(10000 * plane_data.r)); - - printf("\nCalculated outputs: \n R: %8.4f\n P: %8.4f\n Y: %8.4f\n T: %8.4f \n", - att_sp.roll_body, att_sp.pitch_body, - att_sp.yaw_body, att_sp.thrust); - - /************************************************************************************************************/ - } - - /* Calculate the P,Q,R body rates of the aircraft */ - //calc_body_angular_rates(plane_data.roll, plane_data.pitch, plane_data.yaw, - // plane_data.rollspeed, plane_data.pitchspeed, plane_data.yawspeed); - - /* Calculate the body frame angles of the aircraft */ - //calc_bodyframe_angles(plane_data.roll,plane_data.pitch,plane_data.yaw); - - // if (manual.override_mode_switch < XXX) { - - - /* Navigation part */ - - // Get Waypoint - - // XXX FIXME - - //else if (manual.override_mode_switch > MANUAL) { // AUTO mode - - /* calculate setpoint based on current position and waypoint */ - att_sp.roll_body = calc_roll_setpoint(); - att_sp.pitch_body = calc_pitch_setpoint(); - att_sp.thrust = calc_throttle_setpoint(); - - /* calculate the control outputs based on roll / pitch / yaw setpoints */ - actuators.control[0] = calc_roll_ail(); - actuators.control[1] = calc_pitch_elev(); - actuators.control[2] = calc_yaw_rudder(att.yaw); - actuators.control[3] = calc_throttle(); - - /* publish attitude setpoint (for MAVLink) */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - - /* publish actuator setpoints (for mixer) */ - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - - loopcounter++; - - /* 20Hz loop*/ - usleep(50000); - } - - return 0; + param_get(h->heading_p, &(p->heading_p)); + param_get(h->heading_lim, &(p->heading_lim)); + + return OK; } diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 6cf811158d..5bc0d5fa3f 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -1,7 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier + * 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 @@ -36,6 +36,8 @@ * @file multirotor_att_control_main.c * * Implementation of multirotor attitude control main loop. + * + * @author Lorenz Meier */ #include diff --git a/apps/multirotor_pos_control/position_control.c b/apps/multirotor_pos_control/position_control.c index fd731a33de..b98f5bedea 100644 --- a/apps/multirotor_pos_control/position_control.c +++ b/apps/multirotor_pos_control/position_control.c @@ -51,46 +51,6 @@ #include "multirotor_position_control.h" -float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) -{ - double lat_now_rad = lat_now / 180.0 * M_PI; - double lon_now_rad = lon_now / 180.0 * M_PI; - double lat_next_rad = lat_next / 180.0 * M_PI; - double lon_next_rad = lon_next / 180.0 * M_PI; - - - double d_lat = lat_next_rad - lat_now_rad; - double d_lon = lon_next_rad - lon_now_rad; - - double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0) * cos(lat_now_rad) * cos(lat_next_rad); - double c = 2 * atan2(sqrt(a), sqrt(1 - a)); - - const double radius_earth = 6371000.0; - - return radius_earth * c; -} - -float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) -{ - double lat_now_rad = lat_now / 180.0 * M_PI; - double lon_now_rad = lon_now / 180.0 * M_PI; - double lat_next_rad = lat_next / 180.0 * M_PI; - double lon_next_rad = lon_next / 180.0 * M_PI; - - double d_lat = lat_next_rad - lat_now_rad; - double d_lon = lon_next_rad - lon_now_rad; - - /* conscious mix of double and float trig function to maximize speed and efficiency */ - float theta = atan2f(sin(d_lon) * cos(lat_next_rad) , cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon)); - - // XXX wrapping check is incomplete - if (theta < 0.0f) { - theta = theta + 2.0f * M_PI_F; - } - - return theta; -} - void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual, const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos, const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp) diff --git a/apps/systemlib/Makefile b/apps/systemlib/Makefile index ec6bb3fb32..b49a3c54a4 100644 --- a/apps/systemlib/Makefile +++ b/apps/systemlib/Makefile @@ -48,7 +48,8 @@ CSRCS = err.c \ # ifeq ($(TARGET),px4fmu) CSRCS += systemlib.c \ - pid/pid.c + pid/pid.c \ + geo/geo.c endif include $(APPDIR)/mk/app.mk diff --git a/apps/systemlib/geo/geo.c b/apps/systemlib/geo/geo.c new file mode 100644 index 0000000000..42d8d9c150 --- /dev/null +++ b/apps/systemlib/geo/geo.c @@ -0,0 +1,88 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler + * Julian Oes + * 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 geo.c + * + * Geo / math functions to perform geodesic calculations + * + * @author Thomas Gubler + * @author Julian Oes + * @author Lorenz Meier + */ + +#include +#include + +__EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) +{ + double lat_now_rad = lat_now / 180.0 * M_PI; + double lon_now_rad = lon_now / 180.0 * M_PI; + double lat_next_rad = lat_next / 180.0 * M_PI; + double lon_next_rad = lon_next / 180.0 * M_PI; + + + double d_lat = lat_next_rad - lat_now_rad; + double d_lon = lon_next_rad - lon_now_rad; + + double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0) * cos(lat_now_rad) * cos(lat_next_rad); + double c = 2 * atan2(sqrt(a), sqrt(1 - a)); + + const double radius_earth = 6371000.0; + + return radius_earth * c; +} + +__EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) +{ + double lat_now_rad = lat_now / 180.0 * M_PI; + double lon_now_rad = lon_now / 180.0 * M_PI; + double lat_next_rad = lat_next / 180.0 * M_PI; + double lon_next_rad = lon_next / 180.0 * M_PI; + + double d_lat = lat_next_rad - lat_now_rad; + double d_lon = lon_next_rad - lon_now_rad; + + /* conscious mix of double and float trig function to maximize speed and efficiency */ + float theta = atan2f(sin(d_lon) * cos(lat_next_rad) , cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon)); + + // XXX wrapping check is incomplete + if (theta < 0.0f) { + theta = theta + 2.0f * M_PI_F; + } + + return theta; +} \ No newline at end of file diff --git a/apps/systemlib/geo/geo.h b/apps/systemlib/geo/geo.h new file mode 100644 index 0000000000..0e86b3599d --- /dev/null +++ b/apps/systemlib/geo/geo.h @@ -0,0 +1,49 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler + * Julian Oes + * 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 geo.h + * + * Definition of geo / math functions to perform geodesic calculations + * + * @author Thomas Gubler + * @author Julian Oes + * @author Lorenz Meier + */ + +__EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); + +__EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); From 5f7c2b3eade6b23f1788fd4e5bc3f9b9f010c3af Mon Sep 17 00:00:00 2001 From: patacongo Date: Sat, 22 Sep 2012 16:48:44 +0000 Subject: [PATCH 19/56] Shenzhou board uses RMII PHY interface, not MII git-svn-id: http://svn.code.sf.net/p/nuttx/code/trunk@5176 42af7a65-404d-4744-a932-0658087f49c3 --- nuttx/arch/arm/src/stm32/stm32_eth.c | 2 +- nuttx/configs/shenzhou/README.txt | 4 ++-- nuttx/configs/shenzhou/include/board.h | 14 +++++++------- nuttx/configs/shenzhou/nsh/defconfig | 9 ++++++--- 4 files changed, 16 insertions(+), 13 deletions(-) diff --git a/nuttx/arch/arm/src/stm32/stm32_eth.c b/nuttx/arch/arm/src/stm32/stm32_eth.c index 59f3def7f6..51dd7e1862 100644 --- a/nuttx/arch/arm/src/stm32/stm32_eth.c +++ b/nuttx/arch/arm/src/stm32/stm32_eth.c @@ -2854,7 +2854,7 @@ static void stm32_ethreset(FAR struct stm32_ethmac_s *priv) * reset all the registers holds their reset values. */ - regval = stm32_getreg(STM32_ETH_DMABMR); + regval = stm32_getreg(STM32_ETH_DMABMR); regval |= ETH_DMABMR_SR; stm32_putreg(regval, STM32_ETH_DMABMR); diff --git a/nuttx/configs/shenzhou/README.txt b/nuttx/configs/shenzhou/README.txt index e68bd7bf9f..e929f2b8ba 100644 --- a/nuttx/configs/shenzhou/README.txt +++ b/nuttx/configs/shenzhou/README.txt @@ -560,9 +560,9 @@ Shenzhou-specific Configuration Options CONFIG_STM32_PHYADDR - The 5-bit address of the PHY on the board CONFIG_STM32_MII - Support Ethernet MII interface - CONFIG_STM32_MII_MCO1 - Use MCO1 to clock the MII interface - CONFIG_STM32_MII_MCO2 - Use MCO2 to clock the MII interface + CONFIG_STM32_MII_MCO - Use MCO to clock the MII interface CONFIG_STM32_RMII - Support Ethernet RMII interface + CONFIG_STM32_RMII_MCO - Use MCO to clock the RMII interface CONFIG_STM32_AUTONEG - Use PHY autonegotion to determine speed and mode CONFIG_STM32_ETHFD - If CONFIG_STM32_AUTONEG is not defined, then this may be defined to select full duplex mode. Default: half-duplex diff --git a/nuttx/configs/shenzhou/include/board.h b/nuttx/configs/shenzhou/include/board.h index 8e4d895318..df4386cf1b 100644 --- a/nuttx/configs/shenzhou/include/board.h +++ b/nuttx/configs/shenzhou/include/board.h @@ -166,9 +166,9 @@ * -- ---- -------------- ---------------------------------------------------------- * PN NAME SIGNAL NOTES * -- ---- -------------- ---------------------------------------------------------- - * 24 PA1 MII_RX_CLK Ethernet PHY - * RMII_REF_CLK Ethernet PHY - * 25 PA2 MII_MDIO Ethernet PHY + * 24 PA1 MII_RX_CLK Ethernet PHY NOTE: Despite the MII labeling of these + * RMII_REF_CLK Ethernet PHY signals, the DM916AEP is actually configured + * 25 PA2 MII_MDIO Ethernet PHY to work in RMII mode. * 48 PB11 MII_TX_EN Ethernet PHY * 51 PB12 MII_TXD0 Ethernet PHY * 52 PB13 MII_TXD1 Ethernet PHY @@ -186,11 +186,11 @@ # ifndef CONFIG_STM32_ETH_REMAP # error "STM32 Ethernet requires CONFIG_STM32_ETH_REMAP" # endif -# ifndef CONFIG_STM32_MII -# error "STM32 Ethernet requires CONFIG_STM32_MII" +# ifndef CONFIG_STM32_RMII +# error "STM32 Ethernet requires CONFIG_STM32_RMII" # endif -# ifndef CONFIG_STM32_MII_MCO -# error "STM32 Ethernet requires CONFIG_STM32_MII_MCO" +# ifndef CONFIG_STM32_RMII_MCO +# error "STM32 Ethernet requires CONFIG_STM32_RMII_MCO" # endif #endif diff --git a/nuttx/configs/shenzhou/nsh/defconfig b/nuttx/configs/shenzhou/nsh/defconfig index f231b26325..053717470e 100644 --- a/nuttx/configs/shenzhou/nsh/defconfig +++ b/nuttx/configs/shenzhou/nsh/defconfig @@ -161,7 +161,7 @@ CONFIG_STM32_JTAG_FULL_ENABLE=y # CONFIG_STM32_JTAG_NOJNTRST_ENABLE is not set # CONFIG_STM32_JTAG_SW_ENABLE is not set # CONFIG_STM32_FORCEPOWER is not set -# CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG is not set` +# CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG is not set # # SPI Configuration @@ -173,8 +173,8 @@ CONFIG_STM32_JTAG_FULL_ENABLE=y # Ethernet MAC configuration # CONFIG_STM32_PHYADDR=1 -CONFIG_STM32_MII=y -CONFIG_STM32_MII_MCO=y +# CONFIG_STM32_MII is not set +# CONFIG_STM32_MII_MCO is not set # CONFIG_STM32_MII_EXTCLK is not set CONFIG_STM32_AUTONEG=y CONFIG_STM32_PHYSR=16 @@ -183,6 +183,9 @@ CONFIG_STM32_PHYSR_100MBPS=0x0000 CONFIG_STM32_PHYSR_MODE=0x0004 CONFIG_STM32_PHYSR_FULLDUPLEX=0x0004 # CONFIG_STM32_ETH_PTP is not set +CONFIG_STM32_RMII=y +CONFIG_STM32_RMII_MCO=y +# CONFIG_STM32_RMII_EXTCLK is not set # # USB Host Configuration From 8b951ec417454353d61d19b3379e52b6da5dd6b6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Sep 2012 20:55:44 +0200 Subject: [PATCH 20/56] WIP on HIL --- apps/fixedwing_control/fixedwing_control.c | 9 ++- apps/mavlink/mavlink.c | 47 ++++++++-------- .../multirotor_attitude_control.c | 6 +- .../multirotor_rate_control.c | 6 +- apps/systemlib/pid/pid.c | 56 +++++++++++++------ apps/systemlib/pid/pid.h | 5 +- 6 files changed, 78 insertions(+), 51 deletions(-) diff --git a/apps/fixedwing_control/fixedwing_control.c b/apps/fixedwing_control/fixedwing_control.c index 1323f2566d..bdf7976736 100644 --- a/apps/fixedwing_control/fixedwing_control.c +++ b/apps/fixedwing_control/fixedwing_control.c @@ -223,11 +223,11 @@ int fixedwing_control_thread_main(int argc, char *argv[]) PID_t roll_pos_controller; pid_init(&roll_pos_controller, p.roll_pos_p, p.roll_pos_i, 0.0f, p.roll_pos_awu, - PID_MODE_DERIVATIV_SET, 155); + PID_MODE_DERIVATIV_SET); PID_t heading_controller; pid_init(&heading_controller, ppos.heading_p, 0.0f, 0.0f, 0.0f, - PID_MODE_DERIVATIV_SET, 155); + PID_MODE_DERIVATIV_SET); while(!thread_should_exit) { @@ -245,6 +245,8 @@ int fixedwing_control_thread_main(int argc, char *argv[]) // FIXME SUBSCRIBE if (loopcounter % 20 == 0) { + att_parameters_update(&h, &p); + pos_parameters_update(&hpos, &ppos); pid_set_parameters(&roll_pos_controller, p.roll_pos_p, p.roll_pos_i, 0.0f, p.roll_pos_awu); pid_set_parameters(&heading_controller, ppos.heading_p, 0.0f, 0.0f, 0.0f); } @@ -420,6 +422,9 @@ static int att_parameters_init(struct fw_att_control_param_handles *h) static int att_parameters_update(const struct fw_att_control_param_handles *h, struct fw_att_control_params *p) { param_get(h->roll_pos_p, &(p->roll_pos_p)); + param_get(h->roll_pos_i, &(p->roll_pos_i)); + param_get(h->roll_pos_lim, &(p->roll_pos_lim)); + param_get(h->roll_pos_awu, &(p->roll_pos_awu)); return OK; } diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index ce0a238dd0..1f745baac4 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -936,29 +936,6 @@ static void *uorb_receiveloop(void *arg) orb_copy(ORB_ID(vehicle_attitude_setpoint), subs->spa_sub, &buf.att_sp); mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0, buf.att_sp.timestamp/1000, buf.att_sp.roll_body, buf.att_sp.pitch_body, buf.att_sp.yaw_body, buf.att_sp.thrust); - - /* Only send in HIL mode */ - if (mavlink_hil_enabled) { - - /* translate the current syste state to mavlink state and mode */ - uint8_t mavlink_state = 0; - uint8_t mavlink_mode = 0; - get_mavlink_mode_and_state(&v_status, &armed, &mavlink_state, &mavlink_mode); - - /* HIL message as per MAVLink spec */ - mavlink_msg_hil_controls_send(chan, - hrt_absolute_time(), - buf.att_sp.roll_body, /* this may be replaced by ctrl0 later */ - buf.att_sp.pitch_body, - buf.att_sp.yaw_body, - buf.att_sp.thrust, - 0, - 0, - 0, - 0, - mavlink_mode, - 0); - } } /* --- ACTUATOR OUTPUTS 0 --- */ @@ -975,6 +952,7 @@ static void *uorb_receiveloop(void *arg) buf.act_outputs.output[5], buf.act_outputs.output[6], buf.act_outputs.output[7]); + // if (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) { // mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(), // 1 /* port 1 */, @@ -1093,6 +1071,29 @@ static void *uorb_receiveloop(void *arg) mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "ctrl1 ", buf.actuators.control[1]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "ctrl2 ", buf.actuators.control[2]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "ctrl3 ", buf.actuators.control[3]); + + /* Only send in HIL mode */ + if (mavlink_hil_enabled) { + + /* translate the current syste state to mavlink state and mode */ + uint8_t mavlink_state = 0; + uint8_t mavlink_mode = 0; + get_mavlink_mode_and_state(&v_status, &armed, &mavlink_state, &mavlink_mode); + + /* HIL message as per MAVLink spec */ + mavlink_msg_hil_controls_send(chan, + hrt_absolute_time(), + buf.actuators.control[0], + buf.actuators.control[1], + buf.actuators.control[2], + buf.actuators.control[3], + 0, + 0, + 0, + 0, + mavlink_mode, + 0); + } } /* --- DEBUG KEY/VALUE --- */ diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c index d7413913bd..6e2a544389 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -214,11 +214,11 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s // pid_init(&yaw_pos_controller, p.yaw_p, p.yaw_i, p.yaw_d, p.yaw_awu, // PID_MODE_DERIVATIV_SET, 154); pid_init(&yaw_speed_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, p.yawrate_awu, - PID_MODE_DERIVATIV_SET, 155); + PID_MODE_DERIVATIV_SET); pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, p.att_awu, - PID_MODE_DERIVATIV_SET, 156); + PID_MODE_DERIVATIV_SET); pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, p.att_awu, - PID_MODE_DERIVATIV_SET, 157); + PID_MODE_DERIVATIV_SET); initialized = true; } diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c index fefd1f7679..b4eb3469b5 100644 --- a/apps/multirotor_att_control/multirotor_rate_control.c +++ b/apps/multirotor_att_control/multirotor_rate_control.c @@ -157,11 +157,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, parameters_update(&h, &p); pid_init(&yaw_speed_controller, p.yawrate_p, 0, p.yawrate_i, p.yawrate_awu, - PID_MODE_DERIVATIV_SET, 155); + PID_MODE_DERIVATIV_SET); pid_init(&pitch_controller, p.attrate_p, p.attrate_i, 0, p.attrate_awu, - PID_MODE_DERIVATIV_SET, 156); + PID_MODE_DERIVATIV_SET); pid_init(&roll_controller, p.attrate_p, p.attrate_i, 0, p.attrate_awu, - PID_MODE_DERIVATIV_SET, 157); + PID_MODE_DERIVATIV_SET); initialized = true; } diff --git a/apps/systemlib/pid/pid.c b/apps/systemlib/pid/pid.c index 807373c158..fbf3edc337 100644 --- a/apps/systemlib/pid/pid.c +++ b/apps/systemlib/pid/pid.c @@ -40,18 +40,19 @@ */ #include "pid.h" +#include __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, - uint8_t mode, uint8_t plot_i) + uint8_t mode) { pid->kp = kp; pid->ki = ki; pid->kd = kd; pid->intmax = intmax; pid->mode = mode; - pid->plot_i = plot_i; pid->count = 0; pid->saturated = 0; + pid->last_output = 0; pid->sp = 0; pid->error_previous = 0; @@ -63,11 +64,7 @@ __EXPORT void pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float pid->ki = ki; pid->kd = kd; pid->intmax = intmax; - // pid->mode = mode; - - // pid->sp = 0; - // pid->error_previous = 0; - // pid->integral = 0; + // pid->limit = limit; } //void pid_set(PID_t *pid, float sp) @@ -95,6 +92,11 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo goto start */ + if (!isfinite(sp) || !isfinite(val) || !isfinite(val_dot) || !isfinite(dt)) + { + return pid->last_output; + } + float i, d; pid->sp = sp; float error = pid->sp - val; @@ -111,7 +113,7 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo } // Anti-Windup. Needed if we don't use the saturation above. - if (pid->intmax != 0.0) { + if (pid->intmax != 0.0f) { if (i > pid->intmax) { pid->integral = pid->intmax; @@ -122,14 +124,6 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo } else { pid->integral = i; } - - //Send Controller integrals - // Disabled because of new possibilities with debug_vect. - // Now sent in Main Loop at 5 Hz. 26.06.2010 Laurens - // if (pid->plot_i && (pid->count++ % 16 == 0)&&(global_data.param[PARAM_SEND_SLOT_DEBUG_2] == 1)) - // { - // mavlink_msg_debug_send(MAVLINK_COMM_1, pid->plot_i, pid->integral); - // } } if (pid->mode == PID_MODE_DERIVATIV_CALC) { @@ -142,7 +136,33 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo d = 0; } - pid->error_previous = error; + if (pid->kd == 0) { + d = 0; + } - return (error * pid->kp) + (i * pid->ki) + (d * pid->kd); + if (pid->ki == 0) { + i = 0; + } + + if (pid->kp == 0) { + p = 0; + } else { + p = error; + } + + if (isfinite(error)) { + pid->error_previous = error; + } + + float val = (error * pid->kp) + (i * pid->ki) + (d * pid->kd); + + if (isfinite(val)) { + last_output = val; + } + + if (!isfinite(pid->integral)) { + pid->integral = 0; + } + + return pid->last_output; } diff --git a/apps/systemlib/pid/pid.h b/apps/systemlib/pid/pid.h index 83bf09b59f..d4bbcaf31d 100644 --- a/apps/systemlib/pid/pid.h +++ b/apps/systemlib/pid/pid.h @@ -58,13 +58,14 @@ typedef struct { float sp; float integral; float error_previous; + float last_output; + float limit; uint8_t mode; - uint8_t plot_i; uint8_t count; uint8_t saturated; } PID_t; -__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, uint8_t mode, uint8_t plot_i); +__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, uint8_t mode); __EXPORT void pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax); //void pid_set(PID_t *pid, float sp); __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt); From 45da426c19717b6e8c48058e1d67c833c16f44b9 Mon Sep 17 00:00:00 2001 From: patacongo Date: Sat, 22 Sep 2012 19:19:56 +0000 Subject: [PATCH 21/56] STM32 Ethernet, Slightly differ register layout for DM9161AEP PHY git-svn-id: http://svn.code.sf.net/p/nuttx/code/trunk@5177 42af7a65-404d-4744-a932-0658087f49c3 --- nuttx/arch/arm/src/stm32/Kconfig | 90 +++++++++++++++---- nuttx/arch/arm/src/stm32/stm32_eth.c | 81 ++++++++++++++--- .../configs/olimex-stm32-p107/include/board.h | 12 +-- nuttx/configs/shenzhou/include/board.h | 13 ++- nuttx/configs/shenzhou/nsh/defconfig | 14 +-- 5 files changed, 164 insertions(+), 46 deletions(-) diff --git a/nuttx/arch/arm/src/stm32/Kconfig b/nuttx/arch/arm/src/stm32/Kconfig index c7dae44608..26c4b20c91 100644 --- a/nuttx/arch/arm/src/stm32/Kconfig +++ b/nuttx/arch/arm/src/stm32/Kconfig @@ -1684,6 +1684,7 @@ config SDIO_WIDTH_D1_ONLY endmenu +if STM32_ETHMAC menu "Ethernet MAC configuration" config STM32_PHYADDR @@ -1695,7 +1696,6 @@ config STM32_PHYADDR config STM32_MII bool "Use MII interface" default n - depends on STM32_ETHMAC ---help--- Support Ethernet MII interface. @@ -1734,14 +1734,13 @@ endchoice config STM32_AUTONEG bool "Use autonegotiation" default y - depends on STM32_ETHMAC ---help--- Use PHY autonegotiation to determine speed and mode config STM32_ETHFD bool "Full duplex" default n - depends on STM32_ETHMAC && !STM32_AUTONEG + depends on !STM32_AUTONEG ---help--- If STM32_AUTONEG is not defined, then this may be defined to select full duplex mode. Default: half-duplex @@ -1749,61 +1748,104 @@ config STM32_ETHFD config STM32_ETH100MBPS bool "100 Mbps" default n - depends on STM32_ETHMAC && !STM32_AUTONEG + depends on !STM32_AUTONEG ---help--- If STM32_AUTONEG is not defined, then this may be defined to select 100 MBps speed. Default: 10 Mbps config STM32_PHYSR - hex "PHY status register address" + int "PHY Status Register Address (decimal)" depends on STM32_AUTONEG ---help--- This must be provided if STM32_AUTONEG is defined. The PHY status register address may diff from PHY to PHY. This configuration sets the address of the PHY status register. -config STM32_PHYSR_SPEED - hex "PHY speed mask" +config STM32_PHYSR_ALTCONFIG + bool "PHY Status Alternate Bit Layout" + default n depends on STM32_AUTONEG + ---help--- + Different PHYs present speed and mode information in different ways. Some + will present separate information for speed and mode (this is the default). + Those PHYs, for example, may provide a 10/100 Mbps indication and a separate + full/half duplex indication. This options selects an alternative representation + where speed and mode information are combined. This might mean, for example, + separate bits for 10HD, 100HD, 10FD and 100FD. + +config STM32_PHYSR_SPEED + hex "PHY Speed Mask" + depends on STM32_AUTONEG && !STM32_PHYSR_ALTCONFIG ---help--- This must be provided if STM32_AUTONEG is defined. This provides bit mask - indicating 10 or 100MBps speed. + for isolating the 10 or 100MBps speed indication. config STM32_PHYSR_100MBPS - hex "PHY 100Mbps speed value" - depends on STM32_AUTONEG + hex "PHY 100Mbps Speed Value" + depends on STM32_AUTONEG && !STM32_PHYSR_ALTCONFIG ---help--- This must be provided if STM32_AUTONEG is defined. This provides the value of the speed bit(s) indicating 100MBps speed. config STM32_PHYSR_MODE - hex "PHY mode mask" - depends on STM32_AUTONEG + hex "PHY Mode Mask" + depends on STM32_AUTONEG && !STM32_PHYSR_ALTCONFIG ---help--- This must be provided if STM32_AUTONEG is defined. This provide bit mask - indicating full or half duplex modes. + for isolating the full or half duplex mode bits. config STM32_PHYSR_FULLDUPLEX - hex "PHY full duplex mode value" - depends on STM32_AUTONEG + hex "PHY Full Duplex Mode Value" + depends on STM32_AUTONEG && !STM32_PHYSR_ALTCONFIG ---help--- This must be provided if STM32_AUTONEG is defined. This provides the value of the mode bits indicating full duplex mode. +config STM32_PHYSR_ALTMODE + hex "PHY Mode Mask" + depends on STM32_AUTONEG && STM32_PHYSR_ALTCONFIG + ---help--- + This must be provided if STM32_AUTONEG is defined. This provide bit mask + for isolating the speed and full/half duplex mode bits. + +config STM32_PHYSR_10HD + hex "10MHz/Half Duplex Value" + depends on STM32_AUTONEG && STM32_PHYSR_ALTCONFIG + ---help--- + This must be provided if STM32_AUTONEG is defined. This is the value + under the bit mask that represents the 10Mbps, half duplex setting. + +config STM32_PHYSR_100HD + hex "100MHz/Half Duplex Value" + depends on STM32_AUTONEG && STM32_PHYSR_ALTCONFIG + ---help--- + This must be provided if STM32_AUTONEG is defined. This is the value + under the bit mask that represents the 100Mbps, half duplex setting. + +config STM32_PHYSR_10FD + hex "10MHz/Full Duplex Value" + depends on STM32_AUTONEG && STM32_PHYSR_ALTCONFIG + ---help--- + This must be provided if STM32_AUTONEG is defined. This is the value + under the bit mask that represents the 10Mbps, full duplex setting. + +config STM32_PHYSR_100FD + hex "100MHz/Full Duplex Value" + depends on STM32_AUTONEG && STM32_PHYSR_ALTCONFIG + ---help--- + This must be provided if STM32_AUTONEG is defined. This is the value + under the bit mask that represents the 100Mbps, full duplex setting. + config STM32_ETH_PTP bool "Precision Time Protocol (PTP)" default n - depends on STM32_ETHMAC ---help--- Precision Time Protocol (PTP). Not supported but some hooks are indicated with this condition. -endmenu - config STM32_RMII bool default y if !STM32_MII - depends on STM32_ETHMAC choice prompt "RMII clock configuration" @@ -1837,6 +1879,16 @@ config STM32_RMII_EXTCLK endchoice +config STM32_ETHMAC_REGDEBUG + bool "Register-Level Debug" + default n + depends on DEBUG + ---help--- + Enable very low-level register access debug. Depends on DEBUG. + +endmenu +endif + menu "USB Host Configuration" config STM32_OTGFS_RXFIFO_SIZE diff --git a/nuttx/arch/arm/src/stm32/stm32_eth.c b/nuttx/arch/arm/src/stm32/stm32_eth.c index 51dd7e1862..b3392fa7a4 100644 --- a/nuttx/arch/arm/src/stm32/stm32_eth.c +++ b/nuttx/arch/arm/src/stm32/stm32_eth.c @@ -135,17 +135,35 @@ # ifndef CONFIG_STM32_PHYSR # error "CONFIG_STM32_PHYSR must be defined in the NuttX configuration" # endif -# ifndef CONFIG_STM32_PHYSR_SPEED -# error "CONFIG_STM32_PHYSR_SPEED must be defined in the NuttX configuration" -# endif -# ifndef CONFIG_STM32_PHYSR_100MBPS -# error "CONFIG_STM32_PHYSR_100MBPS must be defined in the NuttX configuration" -# endif -# ifndef CONFIG_STM32_PHYSR_MODE -# error "CONFIG_STM32_PHYSR_MODE must be defined in the NuttX configuration" -# endif -# ifndef CONFIG_STM32_PHYSR_FULLDUPLEX -# error "CONFIG_STM32_PHYSR_FULLDUPLEX must be defined in the NuttX configuration" +# ifdef CONFIG_STM32_PHYSR_ALTCONFIG +# ifndef CONFIG_STM32_PHYSR_ALTMODE +# error "CONFIG_STM32_PHYSR_ALTMODE must be defined in the NuttX configuration" +# endif +# ifndef CONFIG_STM32_PHYSR_10HD +# error "CONFIG_STM32_PHYSR_10HD must be defined in the NuttX configuration" +# endif +# ifndef CONFIG_STM32_PHYSR_100HD +# error "CONFIG_STM32_PHYSR_100HD must be defined in the NuttX configuration" +# endif +# ifndef CONFIG_STM32_PHYSR_10FD +# error "CONFIG_STM32_PHYSR_10FD must be defined in the NuttX configuration" +# endif +# ifndef CONFIG_STM32_PHYSR_100FD +# error "CONFIG_STM32_PHYSR_100FD must be defined in the NuttX configuration" +# endif +# else +# ifndef CONFIG_STM32_PHYSR_SPEED +# error "CONFIG_STM32_PHYSR_SPEED must be defined in the NuttX configuration" +# endif +# ifndef CONFIG_STM32_PHYSR_100MBPS +# error "CONFIG_STM32_PHYSR_100MBPS must be defined in the NuttX configuration" +# endif +# ifndef CONFIG_STM32_PHYSR_MODE +# error "CONFIG_STM32_PHYSR_MODE must be defined in the NuttX configuration" +# endif +# ifndef CONFIG_STM32_PHYSR_FULLDUPLEX +# error "CONFIG_STM32_PHYSR_FULLDUPLEX must be defined in the NuttX configuration" +# endif # endif #endif @@ -2552,6 +2570,46 @@ static int stm32_phyinit(FAR struct stm32_ethmac_s *priv) /* Remember the selected speed and duplex modes */ + nvdbg("PHYSR[%d]: %04x\n", CONFIG_STM32_PHYSR, phyval); + + /* Different PHYs present speed and mode information in different ways. IF + * This CONFIG_STM32_PHYSR_ALTCONFIG is selected, this indicates that the PHY + * represents speed and mode information are combined, for example, with + * separate bits for 10HD, 100HD, 10FD and 100FD. + */ + +#ifdef CONFIG_STM32_PHYSR_ALTCONFIG + switch (phyval & CONFIG_STM32_PHYSR_ALTMODE) + { + default: + case CONFIG_STM32_PHYSR_10HD: + priv->fduplex = 0; + priv->mbps100 = 0; + break; + + case CONFIG_STM32_PHYSR_100HD: + priv->fduplex = 0; + priv->mbps100 = 1; + break; + + case CONFIG_STM32_PHYSR_10FD: + priv->fduplex = 1; + priv->mbps100 = 0; + break; + + case CONFIG_STM32_PHYSR_100FD: + priv->fduplex = 1; + priv->mbps100 = 1; + break; + } + + /* Different PHYs present speed and mode information in different ways. Some + * will present separate information for speed and mode (this is the default). + * Those PHYs, for example, may provide a 10/100 Mbps indication and a separate + * full/half duplex indication. + */ + +#else if ((phyval & CONFIG_STM32_PHYSR_MODE) == CONFIG_STM32_PHYSR_FULLDUPLEX) { priv->fduplex = 1; @@ -2561,6 +2619,7 @@ static int stm32_phyinit(FAR struct stm32_ethmac_s *priv) { priv->mbps100 = 1; } +#endif #else /* Auto-negotion not selected */ diff --git a/nuttx/configs/olimex-stm32-p107/include/board.h b/nuttx/configs/olimex-stm32-p107/include/board.h index d531eb1129..42dd4f4d5b 100644 --- a/nuttx/configs/olimex-stm32-p107/include/board.h +++ b/nuttx/configs/olimex-stm32-p107/include/board.h @@ -50,8 +50,6 @@ * Pre-processor Definitions ************************************************************************************/ -#define BOARD_CFGR_MCO_SOURCE RCC_CFGR_PLL3CLK - /* Clocking *************************************************************************/ /* On-board crystal frequency is 25MHz (HSE) */ @@ -97,10 +95,14 @@ #define STM32_APB1_TIM6_CLKIN (STM32_PCLK1_FREQUENCY) #define STM32_APB1_TIM7_CLKIN (STM32_PCLK1_FREQUENCY) -/* MCO output */ - +/* MCO output driven by PLL3. From above, we already have PLL3 input frequency as: + * + * STM32_PLL_PREDIV2 = 5, 25MHz / 5 => 5MHz + */ + #if defined(CONFIG_STM32_MII_MCO) || defined(CONFIG_STM32_RMII_MCO) -# define STM32_PLL_PLL3MUL RCC_CFGR2_PLL3MULx10 +# define BOARD_CFGR_MCO_SOURCE RCC_CFGR_PLL3CLK /* Source: PLL3 */ +# define STM32_PLL_PLL3MUL RCC_CFGR2_PLL3MULx10 /* MCO 5MHz * 10 = 50MHz */ #endif /************************************************************************************ diff --git a/nuttx/configs/shenzhou/include/board.h b/nuttx/configs/shenzhou/include/board.h index df4386cf1b..d396e25631 100644 --- a/nuttx/configs/shenzhou/include/board.h +++ b/nuttx/configs/shenzhou/include/board.h @@ -50,8 +50,6 @@ * Pre-processor Definitions ************************************************************************************/ -#define BOARD_CFGR_MCO_SOURCE RCC_CFGR_PLL3CLK - /* Clocking *************************************************************************/ /* On-board crystal frequency is 25MHz (HSE) */ @@ -97,10 +95,14 @@ #define STM32_APB1_TIM6_CLKIN (STM32_PCLK1_FREQUENCY) #define STM32_APB1_TIM7_CLKIN (STM32_PCLK1_FREQUENCY) -/* MCO output */ +/* MCO output driven by PLL3. From above, we already have PLL3 input frequency as: + * + * STM32_PLL_PREDIV2 = 5, 25MHz / 5 => 5MHz + */ #if defined(CONFIG_STM32_MII_MCO) || defined(CONFIG_STM32_RMII_MCO) -# define STM32_PLL_PLL3MUL RCC_CFGR2_PLL3MULx10 +# define BOARD_CFGR_MCO_SOURCE RCC_CFGR_PLL3CLK /* Source: PLL3 */ +# define STM32_PLL_PLL3MUL RCC_CFGR2_PLL3MULx10 /* MCO 5MHz * 10 = 50MHz */ #endif /* LED definitions ******************************************************************/ @@ -179,6 +181,9 @@ * 56 PD9 MII_RXD0 Ethernet PHY. Requires CONFIG_STM32_ETH_REMAP * 57 PD10 MII_RXD1 Ethernet PHY. Requires CONFIG_STM32_ETH_REMAP * + * The board desdign can support a 50MHz external clock to drive the PHY + * (U9). However, on my board, U9 is not present. + * * 67 PA8 MCO DM9161AEP */ diff --git a/nuttx/configs/shenzhou/nsh/defconfig b/nuttx/configs/shenzhou/nsh/defconfig index 053717470e..c4817c2ee0 100644 --- a/nuttx/configs/shenzhou/nsh/defconfig +++ b/nuttx/configs/shenzhou/nsh/defconfig @@ -174,14 +174,14 @@ CONFIG_STM32_JTAG_FULL_ENABLE=y # CONFIG_STM32_PHYADDR=1 # CONFIG_STM32_MII is not set -# CONFIG_STM32_MII_MCO is not set -# CONFIG_STM32_MII_EXTCLK is not set CONFIG_STM32_AUTONEG=y -CONFIG_STM32_PHYSR=16 -CONFIG_STM32_PHYSR_SPEED=0x0002 -CONFIG_STM32_PHYSR_100MBPS=0x0000 -CONFIG_STM32_PHYSR_MODE=0x0004 -CONFIG_STM32_PHYSR_FULLDUPLEX=0x0004 +CONFIG_STM32_PHYSR=17 +CONFIG_STM32_PHYSR_ALTCONFIG=y +CONFIG_STM32_PHYSR_ALTMODE=0xf000 +CONFIG_STM32_PHYSR_10HD=0x1000 +CONFIG_STM32_PHYSR_100HD=0x4000 +CONFIG_STM32_PHYSR_10FD=0x2000 +CONFIG_STM32_PHYSR_100FD=0x8000 # CONFIG_STM32_ETH_PTP is not set CONFIG_STM32_RMII=y CONFIG_STM32_RMII_MCO=y From c9bb9dd995c571e8406683c6b88b3f757460ffbb Mon Sep 17 00:00:00 2001 From: patacongo Date: Sat, 22 Sep 2012 20:36:36 +0000 Subject: [PATCH 22/56] Adds support for keep-alive connections to webserver git-svn-id: http://svn.code.sf.net/p/nuttx/code/trunk@5178 42af7a65-404d-4744-a932-0658087f49c3 --- apps/ChangeLog.txt | 2 + apps/include/netutils/httpd.h | 3 ++ apps/netutils/webserver/Kconfig | 18 ++++++++ apps/netutils/webserver/httpd.c | 76 +++++++++++++++++++++++++++------ 4 files changed, 86 insertions(+), 13 deletions(-) diff --git a/apps/ChangeLog.txt b/apps/ChangeLog.txt index c5b8dec92d..46ccccc7fb 100644 --- a/apps/ChangeLog.txt +++ b/apps/ChangeLog.txt @@ -338,3 +338,5 @@ Both from Kate. * apps/netutils/webserver/httpd.c: Improvements to HTTP parser from Kate. + * apps/netutils/webserver/httpd.c: Add support for Keep-alive connections + (from Kate). diff --git a/apps/include/netutils/httpd.h b/apps/include/netutils/httpd.h index 0318408023..20ddff2788 100644 --- a/apps/include/netutils/httpd.h +++ b/apps/include/netutils/httpd.h @@ -109,6 +109,9 @@ struct httpd_state { char ht_buffer[HTTPD_IOBUFFER_SIZE]; /* recv() buffer */ char ht_filename[HTTPD_MAX_FILENAME]; /* filename from GET command */ +#ifndef CONFIG_NETUTILS_HTTPD_KEEPALIVE_DISABLE + bool ht_keepalive; /* Connection: keep-alive */ +#endif struct httpd_fs_file ht_file; /* Fake file data to send */ int ht_sockfd; /* The socket descriptor from accept() */ char *ht_scriptptr; diff --git a/apps/netutils/webserver/Kconfig b/apps/netutils/webserver/Kconfig index edca8dfdd2..69a39510d6 100644 --- a/apps/netutils/webserver/Kconfig +++ b/apps/netutils/webserver/Kconfig @@ -117,4 +117,22 @@ config NETUTILS_HTTPD_MMAP representation. endchoice + +config NETUTILS_HTTPD_KEEPALIVE_DISABLE + bool "Keepalive Disable" + default y if !NETUTILS_HTTPD_TIMEOUT + default n if NETUTILS_HTTPD_TIMEOUT + ---help--- + Disabled HTTP keep-alive for HTTP clients. Keep-alive permits a + client to make multiple requests over the same connection, rather + than closing and opening a new socket for each request. + + This depends on the content-length being known, and is automatically + disabled for situations where that header isn't produced (i.e. + scripting, CGI). Keep-alive is also disabled for certain error + responses. + + Keep-alive should normally be disabled if timeouts are enabled, + otherwise a rogue HTTP client could block the httpd indefinitely. + endif diff --git a/apps/netutils/webserver/httpd.c b/apps/netutils/webserver/httpd.c index ebcf300b68..d7dae55da8 100644 --- a/apps/netutils/webserver/httpd.c +++ b/apps/netutils/webserver/httpd.c @@ -104,6 +104,16 @@ # define CONFIG_NETUTILS_HTTPD_TIMEOUT 0 #endif +/* If timeouts are not enabled, then keep-alive is disabled. This is to + * prevent a rogue HTTP client from blocking the httpd indefinitely. + */ + +#if !defined(CONFIG_NETUTILS_HTTPD_KEEPALIVE_DISABLE) +# if CONFIG_NETUTILS_HTTPD_TIMEOUT == 0 +# define CONFIG_NETUTILS_HTTPD_KEEPALIVE_DISABLE +# endif +#endif + #if !defined(CONFIG_NETUTILS_HTTPD_SENDFILE) && !defined(CONFIG_NETUTILS_HTTPD_MMAP) # ifndef CONFIG_NETUTILS_HTTPD_INDEX # ifndef CONFIG_NETUTILS_HTTPD_SCRIPT_DISABLE @@ -372,6 +382,12 @@ static int send_headers(struct httpd_state *pstate, int status, int len) { (void) snprintf(cl, sizeof cl, "Content-Length: %d\r\n", len); } +#ifndef CONFIG_NETUTILS_HTTPD_KEEPALIVE_DISABLE + else + { + pstate->ht_keepalive = false; + } +#endif if (status == 413) { @@ -383,12 +399,17 @@ static int send_headers(struct httpd_state *pstate, int status, int len) #ifndef CONFIG_NETUTILS_HTTPD_SERVERHEADER_DISABLE "Server: uIP/NuttX http://nuttx.org/\r\n" #endif - "Connection: close\r\n" + "Connection: %s\r\n" "Content-type: %s\r\n" "%s" "\r\n", status, status >= 400 ? "Error" : "OK", +#ifndef CONFIG_NETUTILS_HTTPD_KEEPALIVE_DISABLE + pstate->ht_keepalive ? "keep-alive" : "close", +#else + "close", +#endif mime, len >= 0 ? cl : ""); @@ -407,6 +428,13 @@ static int httpd_senderror(struct httpd_state *pstate, int status) status = 500; } +#ifndef CONFIG_NETUTILS_HTTPD_KEEPALIVE_DISABLE + if (status != 404) + { + pstate->ht_keepalive = false; + } +#endif + (void) snprintf(pstate->ht_filename, sizeof pstate->ht_filename, "%s/%d.html", CONFIG_NETUTILS_HTTPD_ERRPATH, status); @@ -456,6 +484,9 @@ static int httpd_sendfile(struct httpd_state *pstate) f = httpd_cgi(pstate->ht_filename); if (f != NULL) { +#ifndef CONFIG_NETUTILS_HTTPD_KEEPALIVE_DISABLE + pstate->ht_keepalive = false; +#endif f(pstate, pstate->ht_filename); return OK; @@ -474,6 +505,9 @@ static int httpd_sendfile(struct httpd_state *pstate) if (ptr != NULL && strncmp(ptr, ".shtml", strlen(".shtml")) == 0) { +#ifndef CONFIG_NETUTILS_HTTPD_KEEPALIVE_DISABLE + pstate->ht_keepalive = false; +#endif if (send_headers(pstate, 200, -1) != OK) { goto done; @@ -594,7 +628,7 @@ static inline int httpd_parse(struct httpd_state *pstate) if (0 != strcmp(v, " HTTP/1.0") && 0 != strcmp(v, " HTTP/1.1")) { - ndbg("[%d] HTTP/%d.%d not supported\n", major, minor); + ndbg("[%d] HTTP version not supported\n"); return 505; } @@ -638,7 +672,12 @@ static inline int httpd_parse(struct httpd_state *pstate) ndbg("[%d] non-zero request length\n"); return 413; } - +#ifndef CONFIG_NETUTILS_HTTPD_KEEPALIVE_DISABLE + else if (0 == strcasecmp(start, "Connection") && 0 == strcasecmp(v, "keep-alive")) + { + pstate->ht_keepalive = true; + } +#endif break; case STATE_BODY: @@ -650,7 +689,7 @@ static inline int httpd_parse(struct httpd_state *pstate) /* Shuffle down for the next block */ memmove(pstate->ht_buffer, start, o - start); - o -= start; + o -= (start - pstate->ht_buffer); } while (state != STATE_BODY); @@ -695,17 +734,28 @@ static void *httpd_handler(void *arg) memset(pstate, 0, sizeof(struct httpd_state)); pstate->ht_sockfd = sockfd; - /* Then handle the next httpd command */ +#ifndef CONFIG_NETUTILS_HTTPD_KEEPALIVE_DISABLE + do + { + pstate->ht_keepalive = false; +#endif - status = httpd_parse(pstate); - if (status >= 400) - { - ret = httpd_senderror(pstate, status); - } - else - { - ret = httpd_sendfile(pstate); + /* Then handle the next httpd command */ + + status = httpd_parse(pstate); + if (status >= 400) + { + ret = httpd_senderror(pstate, status); + } + else + { + ret = httpd_sendfile(pstate); + } + +#ifndef CONFIG_NETUTILS_HTTPD_KEEPALIVE_DISABLE } + while (pstate->ht_keepalive); +#endif /* End of command processing -- Clean up and exit */ From 5b51b5e3a4a0446323c9f2dec527fb9484093c63 Mon Sep 17 00:00:00 2001 From: patacongo Date: Sat, 22 Sep 2012 20:38:43 +0000 Subject: [PATCH 23/56] hpttd.h needs to include stdbool.h git-svn-id: http://svn.code.sf.net/p/nuttx/code/trunk@5179 42af7a65-404d-4744-a932-0658087f49c3 --- apps/include/netutils/httpd.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/apps/include/netutils/httpd.h b/apps/include/netutils/httpd.h index 20ddff2788..2eb0d5bdeb 100644 --- a/apps/include/netutils/httpd.h +++ b/apps/include/netutils/httpd.h @@ -46,6 +46,9 @@ #include +#include +#include + /**************************************************************************** * Pre-processor Definitions ****************************************************************************/ From b58026b8f00d74550279392ae0d83f5acea978bb Mon Sep 17 00:00:00 2001 From: patacongo Date: Sat, 22 Sep 2012 22:25:21 +0000 Subject: [PATCH 24/56] Add missing STM32 F1 pin remapping definitions git-svn-id: http://svn.code.sf.net/p/nuttx/code/trunk@5180 42af7a65-404d-4744-a932-0658087f49c3 --- apps/netutils/webserver/httpd.c | 1 - .../arm/src/stm32/chip/stm32f10xxx_gpio.h | 80 +++++++++++-------- nuttx/arch/arm/src/stm32/stm32_gpio.c | 69 +++++++++------- .../configs/shenzhou/src/shenzhou-internal.h | 28 +++++++ 4 files changed, 111 insertions(+), 67 deletions(-) diff --git a/apps/netutils/webserver/httpd.c b/apps/netutils/webserver/httpd.c index d7dae55da8..f96fc5a6c4 100644 --- a/apps/netutils/webserver/httpd.c +++ b/apps/netutils/webserver/httpd.c @@ -739,7 +739,6 @@ static void *httpd_handler(void *arg) { pstate->ht_keepalive = false; #endif - /* Then handle the next httpd command */ status = httpd_parse(pstate); diff --git a/nuttx/arch/arm/src/stm32/chip/stm32f10xxx_gpio.h b/nuttx/arch/arm/src/stm32/chip/stm32f10xxx_gpio.h index feeeda6dd2..a95d13100e 100644 --- a/nuttx/arch/arm/src/stm32/chip/stm32f10xxx_gpio.h +++ b/nuttx/arch/arm/src/stm32/chip/stm32f10xxx_gpio.h @@ -265,48 +265,58 @@ /* AF remap and debug I/O configuration register */ -#define AFIO_MAPR_SWJ_CFG_SHIFT (24) /* Bits 26-24: Serial Wire JTAG configuration*/ +#define AFIO_MAPR_SPI1_REMAP (1 << 0) /* Bit 0: SPI1 remapping */ +#define AFIO_MAPR_I2C1_REMAP (1 << 1) /* Bit 1: I2C1 remapping */ +#define AFIO_MAPR_USART1_REMAP (1 << 2) /* Bit 2: USART1 remapping */ +#define AFIO_MAPR_USART2_REMAP (1 << 3) /* Bit 3: USART2 remapping */ +#define AFIO_MAPR_USART3_REMAP_SHIFT (4) /* Bits 5-4: USART3 remapping */ +#define AFIO_MAPR_USART3_REMAP_MASK (3 << AFIO_MAPR_USART3_REMAP_SHIFT) +# define AFIO_MAPR_USART3_NOREMAP (0 << AFIO_MAPR_USART3_REMAP_SHIFT) /* 00: No remap */ +# define AFIO_MAPR_USART3_PARTREMAP (1 << AFIO_MAPR_USART3_REMAP_SHIFT) /* 01: Partial remap */ +# define AFIO_MAPR_USART3_FULLREMAP (3 << AFIO_MAPR_USART3_REMAP_SHIFT) /* 11: Full remap */ +#define AFIO_MAPR_TIM1_REMAP_SHIFT (6) /* Bits 7-6: TIM1 remapping */ +#define AFIO_MAPR_TIM1_REMAP_MASK (3 << AFIO_MAPR_TIM1_REMAP_SHIFT) +# define AFIO_MAPR_TIM1_NOREMAP (0 << AFIO_MAPR_TIM1_REMAP_SHIFT) /* 00: No remap */ +# define AFIO_MAPR_TIM1_PARTREMAP (1 << AFIO_MAPR_TIM1_REMAP_SHIFT) /* 01: Partial remap */ +# define AFIO_MAPR_TIM1_FULLREMAP (3 << AFIO_MAPR_TIM1_REMAP_SHIFT) /* 11: Full remap */ +#define AFIO_MAPR_TIM2_REMAP_SHIFT (8) /* Bits 9-8: TIM2 remapping */ +#define AFIO_MAPR_TIM2_REMAP_MASK (3 << AFIO_MAPR_TIM2_REMAP_SHIFT) +# define AFIO_MAPR_TIM2_NOREMAP (0 << AFIO_MAPR_TIM2_REMAP_SHIFT) /* 00: No remap */ +# define AFIO_MAPR_TIM2_PARTREMAP1 (1 << AFIO_MAPR_TIM2_REMAP_SHIFT) /* 01: Partial remap */ +# define AFIO_MAPR_TIM2_PARTREMAP2 (2 << AFIO_MAPR_TIM2_REMAP_SHIFT) /* 10: Partial remap */ +# define AFIO_MAPR_TIM2_FULLREMAP (3 << AFIO_MAPR_TIM2_REMAP_SHIFT) /* 11: Full remap */ +#define AFIO_MAPR_TIM3_REMAP_SHIFT (10) /* Bits 11-10: TIM3 remapping */ +#define AFIO_MAPR_TIM3_REMAP_MASK (3 << AFIO_MAPR_TIM3_REMAP_SHIFT) +# define AFIO_MAPR_TIM3_NOREMAP (0 << AFIO_MAPR_TIM3_REMAP_SHIFT) /* 00: No remap */ +# define AFIO_MAPR_TIM3_PARTREMAP (2 << AFIO_MAPR_TIM3_REMAP_SHIFT) /* 10: Partial remap */ +# define AFIO_MAPR_TIM3_FULLREMAP (3 << AFIO_MAPR_TIM3_REMAP_SHIFT) /* 11: Full remap */ +#define AFIO_MAPR_TIM4_REMAP (1 << 12) /* Bit 12: TIM4 remapping */ +#define AFIO_MAPR_CAN1_REMAP_SHIFT (13) /* Bits 14-13: CAN Alternate function remapping */ +#define AFIO_MAPR_CAN1_REMAP_MASK (3 << AFIO_MAPR_CAN1_REMAP_SHIFT) +# define AFIO_MAPR_PA1112 (0 << AFIO_MAPR_CAN1_REMAP_SHIFT) /* 00: CANRX mapped to PA11, CANTX mapped to PA12 */ +# define AFIO_MAPR_PB89 (2 << AFIO_MAPR_CAN1_REMAP_SHIFT) /* 10: CANRX mapped to PB8, CANTX mapped to PB9 */ +# define AFIO_MAPR_PD01 (3 << AFIO_MAPR_CAN1_REMAP_SHIFT) /* 11: CANRX mapped to PD0, CANTX mapped to PD1 */ +#define AFIO_MAPR_PD01_REMAP (1 << 15) /* Bit 15 : Port D0/Port D1 mapping on OSC_IN/OSC_OUT */ +#define AFIO_MAPR_TIM5CH4_IREMAP (1 << 16) /* Bit 16: TIM5 channel4 internal remap */ + /* Bits 17-20: Reserved */ +#ifdef CONFIG_STM32_CONNECTIVITYLINE +# define AFIO_MAPR_ETH_REMAP (1 << 21) /* Bit 21: Ethernet MAC I/O remapping */ +# define AFIO_MAPR_CAN2_REMAP (1 << 22) /* Bit 22: CAN2 I/O remapping */ +# define AFIO_MAPR_MII_RMII_SEL (1 << 23) /* Bit 23: MII or RMII selection */ +#endif +#define AFIO_MAPR_SWJ_CFG_SHIFT (24) /* Bits 26-24: Serial Wire JTAG configuration */ #define AFIO_MAPR_SWJ_CFG_MASK (7 << AFIO_MAPR_SWJ_CFG_SHIFT) # define AFIO_MAPR_SWJRST (0 << AFIO_MAPR_SWJ_CFG_SHIFT) /* 000: Full SWJ (JTAG-DP + SW-DP): Reset State */ # define AFIO_MAPR_SWJ (1 << AFIO_MAPR_SWJ_CFG_SHIFT) /* 001: Full SWJ (JTAG-DP + SW-DP) but without JNTRST */ # define AFIO_MAPR_SWDP (2 << AFIO_MAPR_SWJ_CFG_SHIFT) /* 010: JTAG-DP Disabled and SW-DP Enabled */ # define AFIO_MAPR_DISAB (4 << AFIO_MAPR_SWJ_CFG_SHIFT) /* 100: JTAG-DP Disabled and SW-DP Disabled */ + /* Bit 27: Reserved */ #ifdef CONFIG_STM32_CONNECTIVITYLINE -# define AFIO_MAPR_MII_RMII_SEL (1 << 23) /* MII or RMII selection */ +# define AFIO_MAPR_SPI3_REMAP (1 << 28) /* Bit 28: SPI3 remapping */ +# define AFIO_MAPR_TIM2ITR1_IREMAP (1 << 29) /* Bit 29: TIM2 internal trigger 1 remapping */ +# define AFIO_MAPR_PTP_PPS_REMAP (1 << 30) /* Bit 30: Ethernet PTP PPS remapping */ #endif -#define AFIO_MAPR_PD01_REMAP (1 << 15) /* Bit 15 : Port D0/Port D1 mapping on OSC_IN/OSC_OUT */ -#define AFIO_MAPR_CAN_REMAP_SHIFT (13) /* Bits 14-13: CAN Alternate function remapping */ -#define AFIO_MAPR_CAN_REMAP_MASK (3 << AFIO_MAPR_CAN_REMAP_SHIFT) -# define AFIO_MAPR_PA1112 (0 << AFIO_MAPR_CAN_REMAP_SHIFT) /* 00: CANRX mapped to PA11, CANTX mapped to PA12 */ -# define AFIO_MAPR_PB89 (2 << AFIO_MAPR_CAN_REMAP_SHIFT) /* 10: CANRX mapped to PB8, CANTX mapped to PB9 */ -# define AFIO_MAPR_PD01 (3 << AFIO_MAPR_CAN_REMAP_SHIFT) /* 11: CANRX mapped to PD0, CANTX mapped to PD1 */ -#define AFIO_MAPR_TIM4_REMAP (1 << 12) /* Bit 12: TIM4 remapping */ -#define AFIO_MAPR_TIM3_REMAP_SHIFT (10) /* Bits 11-10: TIM3 remapping */ -#define AFIO_MAPR_TIM3_REMAP_MASK (3 << AFIO_MAPR_TIM3_REMAP_SHIFT) -# define AFIO_MAPR_TIM3_NOREMAP (0 << AFIO_MAPR_TIM3_REMAP_SHIFT) /* 00: No remap (CH1/PA6, CH2/PA7, CH3/PB0, CH4/PB1) */ -# define AFIO_MAPR_TIM3_PARTREMAP (2 << AFIO_MAPR_TIM3_REMAP_SHIFT) /* 10: Partial remap (CH1/PB4, CH2/PB5, CH3/PB0, CH4/PB1) */ -# define AFIO_MAPR_TIM3_FULLREMAP (3 << AFIO_MAPR_TIM3_REMAP_SHIFT) /* 11: Full remap (CH1/PC6, CH2/PC7, CH3/PC8, CH4/PC9) */ -#define AFIO_MAPR_TIM2_REMAP_SHIFT (8) /* Bits 9-8: TIM2 remapping */ -#define AFIO_MAPR_TIM2_REMAP_MASK (3 << AFIO_MAPR_TIM2_REMAP_SHIFT) -# define AFIO_MAPR_TIM2_NOREMAP (0 << AFIO_MAPR_TIM2_REMAP_SHIFT) /* 00: No remap (CH1/ETR/PA0, CH2/PA1, CH3/PA2, CH4/PA3) */ -# define AFIO_MAPR_TIM2_PARTREMAP1 (1 << AFIO_MAPR_TIM2_REMAP_SHIFT) /* 01: Partial remap (CH1/ETR/PA15, CH2/PB3, CH3/PA2, CH4/PA3) */ -# define AFIO_MAPR_TIM2_PARTREMAP2 (2 << AFIO_MAPR_TIM2_REMAP_SHIFT) /* 10: Partial remap (CH1/ETR/PA0, CH2/PA1, CH3/PB10, CH4/PB11) */ -# define AFIO_MAPR_TIM2_FULLREMAP (3 << AFIO_MAPR_TIM2_REMAP_SHIFT) /* 11: Full remap (CH1/ETR/PA15, CH2/PB3, CH3/PB10, CH4/PB11) */ -#define AFIO_MAPR_TIM1_REMAP_SHIFT (6) /* Bits 7-6: TIM1 remapping */ -#define AFIO_MAPR_TIM1_REMAP_MASK (3 << AFIO_MAPR_TIM1_REMAP_SHIFT) -# define AFIO_MAPR_TIM1_NOREMAP (0 << AFIO_MAPR_TIM1_REMAP_SHIFT) /* 00: No remap (ETR/PA12, CH1/PA8, CH2/PA9, CH3/PA10, CH4/PA11, BKIN/PB12, CH1N/PB13, CH2N/PB14, CH3N/PB15) */ -# define AFIO_MAPR_TIM1_PARTREMAP (1 << AFIO_MAPR_TIM1_REMAP_SHIFT) /* 01: Partial remap (ETR/PA12, CH1/PA8, CH2/PA9, CH3/PA10, CH4/PA11, BKIN/PA6, CH1N/PA7, CH2N/PB0, CH3N/PB1) */ -# define AFIO_MAPR_TIM1_FULLREMAP (3 << AFIO_MAPR_TIM1_REMAP_SHIFT) /* 11: Full remap (ETR/PE7, CH1/PE9, CH2/PE11, CH3/PE13, CH4/PE14, BKIN/PE15, CH1N/PE8, CH2N/PE10, CH3N/PE12) */ -#define AFIO_MAPR_USART3_REMAP_SHIFT (6) /* Bits 5-4: USART3 remapping */ -#define AFIO_MAPR_USART3_REMAP_MASK (3 << AFIO_MAPR_USART3_REMAP_SHIFT) -# define AFIO_MAPR_USART3_NOREMAP (0 << AFIO_MAPR_USART3_REMAP_SHIFT) /* 00: No remap (TX/PB10, RX/PB11, CK/PB12, CTS/PB13, RTS/PB14) */ -# define AFIO_MAPR_USART3_PARTREMAP (1 << AFIO_MAPR_USART3_REMAP_SHIFT) /* 01: Partial remap (TX/PC10, RX/PC11, CK/PC12, CTS/PB13, RTS/PB14) */ -# define AFIO_MAPR_USART3_FULLREMAP (3 << AFIO_MAPR_USART3_REMAP_SHIFT) /* 11: Full remap (TX/PD8, RX/PD9, CK/PD10, CTS/PD11, RTS/PD12) */ -#define AFIO_MAPR_USART2_REMAP (1 << 3) /* Bit 3: USART2 remapping */ -#define AFIO_MAPR_USART1_REMAP (1 << 2) /* Bit 2: USART1 remapping */ -#define AFIO_MAPR_I2C1_REMAP (1 << 1) /* Bit 1: I2C1 remapping */ -#define AFIO_MAPR_SPI1_REMAP (1 << 0) /* Bit 0: SPI1 remapping */ - + /* Bit 31: Reserved */ /* External interrupt configuration register 1 */ #define AFIO_EXTICR_PORT_MASK (0x0f) diff --git a/nuttx/arch/arm/src/stm32/stm32_gpio.c b/nuttx/arch/arm/src/stm32/stm32_gpio.c index 143e48a2c8..4703e82089 100644 --- a/nuttx/arch/arm/src/stm32/stm32_gpio.c +++ b/nuttx/arch/arm/src/stm32/stm32_gpio.c @@ -124,14 +124,27 @@ static inline void stm32_gpioremap(void) uint32_t val = 0; -#ifdef CONFIG_STM32_JTAG_FULL_ENABLE - /* The reset default */ -#elif CONFIG_STM32_JTAG_NOJNTRST_ENABLE - val |= AFIO_MAPR_SWJ; /* enabled but without JNTRST */ -#elif CONFIG_STM32_JTAG_SW_ENABLE - val |= AFIO_MAPR_SWDP; /* set JTAG-DP disabled and SW-DP enabled */ -#else - val |= AFIO_MAPR_DISAB; /* set JTAG-DP and SW-DP Disabled */ +#ifdef CONFIG_STM32_SPI1_REMAP + val |= AFIO_MAPR_SPI1_REMAP; +#endif +#ifdef CONFIG_STM32_SPI3_REMAP +#endif + +#ifdef CONFIG_STM32_I2C1_REMAP + val |= AFIO_MAPR_I2C1_REMAP; +#endif + +#ifdef CONFIG_STM32_USART1_REMAP + val |= AFIO_MAPR_USART1_REMAP; +#endif +#ifdef CONFIG_STM32_USART2_REMAP + val |= AFIO_MAPR_USART2_REMAP; +#endif +#ifdef CONFIG_STM32_USART3_FULL_REMAP + val |= AFIO_MAPR_USART3_FULLREMAP; +#endif +#ifdef CONFIG_STM32_USART3_PARTIAL_REMAP + val |= AFIO_MAPR_USART3_PARTREMAP; #endif #ifdef CONFIG_STM32_TIM1_FULL_REMAP @@ -159,35 +172,29 @@ static inline void stm32_gpioremap(void) val |= AFIO_MAPR_TIM4_REMAP; #endif -#ifdef CONFIG_STM32_USART1_REMAP - val |= AFIO_MAPR_USART1_REMAP; -#endif -#ifdef CONFIG_STM32_USART2_REMAP - val |= AFIO_MAPR_USART2_REMAP; -#endif -#ifdef CONFIG_STM32_USART3_FULL_REMAP - val |= AFIO_MAPR_USART3_FULLREMAP; -#endif -#ifdef CONFIG_STM32_USART3_PARTIAL_REMAP - val |= AFIO_MAPR_USART3_PARTREMAP; -#endif - -#ifdef CONFIG_STM32_SPI1_REMAP - val |= AFIO_MAPR_SPI1_REMAP; -#endif -#ifdef CONFIG_STM32_SPI3_REMAP -#endif - -#ifdef CONFIG_STM32_I2C1_REMAP - val |= AFIO_MAPR_I2C1_REMAP; -#endif - #ifdef CONFIG_STM32_CAN1_REMAP1 val |= AFIO_MAPR_PB89; #endif #ifdef CONFIG_STM32_CAN1_REMAP2 val |= AFIO_MAPR_PD01; #endif +#ifdef CONFIG_STM32_CAN2_REMAP /* Connectivity line only */ + val |= AFIO_MAPR_CAN2_REMAP; +#endif + +#ifdef CONFIG_STM32_ETH_REMAP /* Connectivity line only */ + val |= AFIO_MAPR_ETH_REMAP; +#endif + +#ifdef CONFIG_STM32_JTAG_FULL_ENABLE + /* The reset default */ +#elif CONFIG_STM32_JTAG_NOJNTRST_ENABLE + val |= AFIO_MAPR_SWJ; /* enabled but without JNTRST */ +#elif CONFIG_STM32_JTAG_SW_ENABLE + val |= AFIO_MAPR_SWDP; /* set JTAG-DP disabled and SW-DP enabled */ +#else + val |= AFIO_MAPR_DISAB; /* set JTAG-DP and SW-DP Disabled */ +#endif putreg32(val, STM32_AFIO_MAPR); #endif diff --git a/nuttx/configs/shenzhou/src/shenzhou-internal.h b/nuttx/configs/shenzhou/src/shenzhou-internal.h index 48b06e7b0a..b5fb4e35c0 100644 --- a/nuttx/configs/shenzhou/src/shenzhou-internal.h +++ b/nuttx/configs/shenzhou/src/shenzhou-internal.h @@ -65,6 +65,34 @@ /* Shenzhou GPIO Configuration **********************************************************************/ /* STM3240G-EVAL GPIOs ******************************************************************************/ +/* Ethernet + * + * -- ---- -------------- ---------------------------------------------------------- + * PN NAME SIGNAL NOTES + * -- ---- -------------- ---------------------------------------------------------- + * 24 PA1 MII_RX_CLK Ethernet PHY NOTE: Despite the MII labeling of these + * RMII_REF_CLK Ethernet PHY signals, the DM916AEP is actually configured + * 25 PA2 MII_MDIO Ethernet PHY to work in RMII mode. + * 48 PB11 MII_TX_EN Ethernet PHY + * 51 PB12 MII_TXD0 Ethernet PHY + * 52 PB13 MII_TXD1 Ethernet PHY + * 16 PC1 MII_MDC Ethernet PHY + * 34 PC5 MII_INT Ethernet PHY + * 55 PD8 MII_RX_DV Ethernet PHY. Requires CONFIG_STM32_ETH_REMAP + * 55 PD8 RMII_CRSDV Ethernet PHY. Requires CONFIG_STM32_ETH_REMAP + * 56 PD9 MII_RXD0 Ethernet PHY. Requires CONFIG_STM32_ETH_REMAP + * 57 PD10 MII_RXD1 Ethernet PHY. Requires CONFIG_STM32_ETH_REMAP + * + * The board desdign can support a 50MHz external clock to drive the PHY + * (U9). However, on my board, U9 is not present. + * + * 67 PA8 MCO DM9161AEP + */ + +#ifdef CONFIG_STM32_ETHMAC +# define GPIO_MII_INT (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_EXTI|GPIO_PORTC|GPIO_PIN5) +#endif + /* Wireless * * -- ---- -------------- ------------------------------------------------------------------- From de530d6ba1fcbcaf65fc78ac8cca3286fa52d624 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Sep 2012 01:20:41 +0200 Subject: [PATCH 25/56] General robustness improvements in PID struct, numerically close to bullet-proof, error reporting needs improvements still. --- apps/mavlink/mavlink.c | 16 ++++++------ apps/systemlib/pid/pid.c | 53 +++++++++++++++++++++++++++++----------- apps/systemlib/pid/pid.h | 2 +- 3 files changed, 48 insertions(+), 23 deletions(-) diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 1f745baac4..bf0ed346ec 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -1415,10 +1415,10 @@ void handleMessage(mavlink_message_t *msg) memset(&rc_hil, 0, sizeof(rc_hil)); static orb_advert_t rc_pub = 0; - rc_hil.chan[0].raw = 1510 + man.x * 500; - rc_hil.chan[1].raw = 1520 + man.y * 500; - rc_hil.chan[2].raw = 1590 + man.r * 500; - rc_hil.chan[3].raw = 1420 + man.z * 500; + rc_hil.chan[0].raw = 1510 + man.x / 2; + rc_hil.chan[1].raw = 1520 + man.y / 2; + rc_hil.chan[2].raw = 1590 + man.r / 2; + rc_hil.chan[3].raw = 1420 + man.z / 2; rc_hil.chan[0].scaled = man.x; rc_hil.chan[1].scaled = man.y; @@ -1428,10 +1428,10 @@ void handleMessage(mavlink_message_t *msg) struct manual_control_setpoint_s mc; static orb_advert_t mc_pub = 0; - mc.roll = man.x*1000; - mc.pitch = man.y*1000; - mc.yaw = man.r*1000; - mc.roll = man.z*1000; + mc.roll = man.x / 1000.0f; + mc.pitch = man.y / 1000.0f; + mc.yaw = man.r / 1000.0f; + mc.roll = man.z / 1000.0f; /* fake RC channels with manual control input from simulator */ diff --git a/apps/systemlib/pid/pid.c b/apps/systemlib/pid/pid.c index fbf3edc337..61dd5757fb 100644 --- a/apps/systemlib/pid/pid.c +++ b/apps/systemlib/pid/pid.c @@ -58,13 +58,36 @@ __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, pid->error_previous = 0; pid->integral = 0; } -__EXPORT void pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax) +__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax) { - pid->kp = kp; - pid->ki = ki; - pid->kd = kd; - pid->intmax = intmax; + int ret = 0; + + if (isfinite(kp)) { + pid->kp = kp; + } else { + ret = 1; + } + + if (isfinite(ki)) { + pid->ki = ki; + } else { + ret = 1; + } + + if (isfinite(kd)) { + pid->kd = kd; + } else { + ret = 1; + } + + if (isfinite(intmax)) { + pid->intmax = intmax; + } else { + ret = 1; + } + // pid->limit = limit; + return ret; } //void pid_set(PID_t *pid, float sp) @@ -133,19 +156,21 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo d = -val_dot; } else { - d = 0; + d = 0.0f; } - if (pid->kd == 0) { - d = 0; + if (pid->kd == 0.0f) { + d = 0.0f; } - if (pid->ki == 0) { + if (pid->ki == 0.0f) { i = 0; } - if (pid->kp == 0) { - p = 0; + float p; + + if (pid->kp == 0.0f) { + p = 0.0f; } else { p = error; } @@ -154,10 +179,10 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo pid->error_previous = error; } - float val = (error * pid->kp) + (i * pid->ki) + (d * pid->kd); + float output = (error * pid->kp) + (i * pid->ki) + (d * pid->kd); - if (isfinite(val)) { - last_output = val; + if (isfinite(output)) { + pid->last_output = output; } if (!isfinite(pid->integral)) { diff --git a/apps/systemlib/pid/pid.h b/apps/systemlib/pid/pid.h index d4bbcaf31d..098b2bef81 100644 --- a/apps/systemlib/pid/pid.h +++ b/apps/systemlib/pid/pid.h @@ -66,7 +66,7 @@ typedef struct { } PID_t; __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, uint8_t mode); -__EXPORT void pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax); +__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax); //void pid_set(PID_t *pid, float sp); __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt); From 705172d302b99df7ec3c4172c247c6136adbec88 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Sep 2012 12:11:46 +0200 Subject: [PATCH 26/56] Untested, but fully implemented attitude and/or inner rate control --- .../multirotor_att_control_main.c | 130 ++++++++++++------ .../multirotor_attitude_control.c | 20 ++- .../multirotor_attitude_control.h | 5 +- 3 files changed, 106 insertions(+), 49 deletions(-) diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 5bc0d5fa3f..21c720096e 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -56,6 +56,7 @@ #include #include #include +#include #include #include #include @@ -72,16 +73,66 @@ __EXPORT int multirotor_att_control_main(int argc, char *argv[]); - -static enum { - CONTROL_MODE_RATES = 0, - CONTROL_MODE_ATTITUDE = 1, -} control_mode; - - static bool thread_should_exit; static int mc_task; static bool motor_test_mode = false; +static struct actuator_controls_s actuators; +static orb_advert_t actuator_pub; + +/** + * Perform rate control right after gyro reading + */ +static void *rate_control_thread_main(void *arg) +{ + prctl(PR_SET_NAME, "mc rate control", getpid()); + + int gyro_sub = orb_subscribe(ORB_ID(sensor_gyro)); + int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); + + struct pollfd fds = { .fd = gyro_sub, .events = POLLIN }; + + struct gyro_report gyro_report; + struct vehicle_rates_setpoint_s rates_sp; + memset(&rates_sp, 0, sizeof(rates_sp)); + float gyro_lp[3] = {0.0f, 0.0f, 0.0f}; + + while (!thread_should_exit) { + /* rate control at maximum rate */ + /* wait for a sensor update, check for exit condition every 1000 ms */ + int ret = poll(&fds, 1, 1000); + + if (ret < 0) { + /* XXX this is seriously bad - should be an emergency */ + } else if (ret == 0) { + /* XXX this means no sensor data - should be critical or emergency */ + printf("[mc att control] WARNING: Not getting gyro data, no rate control\n"); + } else { + /* get data */ + orb_copy(ORB_ID(sensor_gyro), gyro_sub, &gyro_report); + bool rates_sp_valid = false; + orb_check(rates_sp_sub, &rates_sp_valid); + if (rates_sp_valid) { + orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp); + } + + /* perform local lowpass */ + + /* apply controller */ + // if (state.flag_control_rates_enabled) { + /* lowpass gyros */ + // XXX + gyro_lp[0] = gyro_report.x; + gyro_lp[1] = gyro_report.y; + gyro_lp[2] = gyro_report.z; + + multirotor_control_rates(&rates_sp, gyro_lp, &actuators); + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + // } + } + } + + return NULL; +} static int mc_thread_main(int argc, char *argv[]) @@ -102,8 +153,6 @@ mc_thread_main(int argc, char *argv[]) struct vehicle_rates_setpoint_s rates_sp; memset(&rates_sp, 0, sizeof(rates_sp)); - struct actuator_controls_s actuators; - /* subscribe to attitude, motor setpoints and system state */ int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); @@ -127,8 +176,9 @@ mc_thread_main(int argc, char *argv[]) actuators.control[i] = 0.0f; } - orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); + actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); + orb_advert_t rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); /* register the perf counter */ perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control"); @@ -136,6 +186,13 @@ mc_thread_main(int argc, char *argv[]) /* welcome user */ printf("[multirotor_att_control] starting\n"); + /* ready, spawn pthread */ + pthread_attr_t rate_control_attr; + pthread_attr_init(&rate_control_attr); + pthread_attr_setstacksize(&rate_control_attr, 2048); + pthread_t rate_control_thread; + pthread_create(&rate_control_thread, &rate_control_attr, rate_control_thread_main, NULL); + while (!thread_should_exit) { /* wait for a sensor update, check for exit condition every 500 ms */ @@ -170,16 +227,21 @@ mc_thread_main(int argc, char *argv[]) att_sp.yaw_body = manual.yaw; // XXX Hack, remove, switch to yaw rate controller /* set yaw rate */ rates_sp.yaw = manual.yaw; + att_sp.thrust = manual.throttle; att_sp.timestamp = hrt_absolute_time(); + /* STEP 2: publish the result to the vehicle actuators */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); if (motor_test_mode) { att_sp.roll_body = 0.0f; att_sp.pitch_body = 0.0f; att_sp.yaw_body = 0.0f; att_sp.thrust = 0.1f; - } else { - att_sp.thrust = manual.throttle; + att_sp.timestamp = hrt_absolute_time(); + /* STEP 2: publish the result to the vehicle actuators */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); } + } else if (state.flag_control_offboard_enabled) { /* offboard inputs */ if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) { @@ -187,38 +249,32 @@ mc_thread_main(int argc, char *argv[]) rates_sp.pitch = offboard_sp.p2; rates_sp.yaw = offboard_sp.p3; rates_sp.thrust = offboard_sp.p4; + rates_sp.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); } else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) { att_sp.roll_body = offboard_sp.p1; att_sp.pitch_body = offboard_sp.p2; att_sp.yaw_body = offboard_sp.p3; att_sp.thrust = offboard_sp.p4; + att_sp.timestamp = hrt_absolute_time(); + /* STEP 2: publish the result to the vehicle actuators */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); } /* decide wether we want rate or position input */ } - /** STEP 2: Identify the controller setup to run and set up the inputs correctly */ + /** STEP 3: Identify the controller setup to run and set up the inputs correctly */ /* run attitude controller */ - if (state.flag_control_attitude_enabled) { - multirotor_control_attitude(&att_sp, &att, &actuators); + if (state.flag_control_attitude_enabled && !state.flag_control_rates_enabled) { + multirotor_control_attitude(&att_sp, &att, NULL, &actuators); + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + } else if (state.flag_control_attitude_enabled && state.flag_control_rates_enabled) { + multirotor_control_attitude(&att_sp, &att, &rates_sp, NULL); + orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); } - /* XXX could be also run in an independent loop */ - if (state.flag_control_rates_enabled) { - /* lowpass gyros */ - // XXX - static float gyro_lp[3] = {0, 0, 0}; - gyro_lp[0] = raw.gyro_rad_s[0]; - gyro_lp[1] = raw.gyro_rad_s[1]; - gyro_lp[2] = raw.gyro_rad_s[2]; - - multirotor_control_rates(&rates_sp, gyro_lp, &actuators); - } - - /* STEP 3: publish the result to the vehicle actuators */ - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); perf_end(mc_loop_perf); } @@ -240,6 +296,8 @@ mc_thread_main(int argc, char *argv[]) perf_print_counter(mc_loop_perf); perf_free(mc_loop_perf); + pthread_join(rate_control_thread, NULL); + fflush(stdout); exit(0); } @@ -258,22 +316,10 @@ usage(const char *reason) int multirotor_att_control_main(int argc, char *argv[]) { int ch; - - control_mode = CONTROL_MODE_RATES; unsigned int optioncount = 0; while ((ch = getopt(argc, argv, "tm:")) != EOF) { switch (ch) { - case 'm': - if (!strcmp(optarg, "rates")) { - control_mode = CONTROL_MODE_RATES; - } else if (!strcmp(optarg, "attitude")) { - control_mode = CONTROL_MODE_RATES; - } else { - usage("unrecognized -m value"); - } - optioncount += 2; - break; case 't': motor_test_mode = true; optioncount += 1; diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c index 6e2a544389..2129915d12 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -188,7 +188,7 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc } void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, - const struct vehicle_attitude_s *att, struct actuator_controls_s *actuators) + const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, struct actuator_controls_s *actuators) { static uint64_t last_run = 0; const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; @@ -305,10 +305,20 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s roll_controller.saturated = 1; } - actuators->control[0] = roll_control; - actuators->control[1] = pitch_control; - actuators->control[2] = yaw_rate_control; - actuators->control[3] = motor_thrust; + if (actuators) { + actuators->control[0] = roll_control; + actuators->control[1] = pitch_control; + actuators->control[2] = yaw_rate_control; + actuators->control[3] = motor_thrust; + } + + // XXX change yaw rate to yaw pos controller + if (rates_sp) { + rates_sp->roll = roll_control; + rates_sp->pitch = pitch_control; + rates_sp->yaw = yaw_rate_control; + rates_sp->thrust = motor_thrust; + } motor_skip_counter++; } diff --git a/apps/multirotor_att_control/multirotor_attitude_control.h b/apps/multirotor_att_control/multirotor_attitude_control.h index aa8d27369b..d9d3c04447 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.h +++ b/apps/multirotor_att_control/multirotor_attitude_control.h @@ -48,9 +48,10 @@ #include #include #include +#include #include -void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, - struct actuator_controls_s *actuators); +void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, + const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, struct actuator_controls_s *actuators); #endif /* MULTIROTOR_ATTITUDE_CONTROL_H_ */ From 8da267c51855c21067b258bffcfa479ae52274c7 Mon Sep 17 00:00:00 2001 From: patacongo Date: Sun, 23 Sep 2012 15:22:27 +0000 Subject: [PATCH 27/56] Shenzhou PHY address should be 0; make sure the F2/F4 bits are not set when using STM32 ethernet driver with F1 git-svn-id: http://svn.code.sf.net/p/nuttx/code/trunk@5181 42af7a65-404d-4744-a932-0658087f49c3 --- nuttx/arch/arm/src/stm32/chip/stm32_eth.h | 27 +++++++++++++---- nuttx/arch/arm/src/stm32/stm32_eth.c | 35 ++++++++++++++++------- nuttx/configs/shenzhou/nsh/defconfig | 2 +- nuttx/drivers/net/enc28j60.h | 2 +- 4 files changed, 49 insertions(+), 17 deletions(-) diff --git a/nuttx/arch/arm/src/stm32/chip/stm32_eth.h b/nuttx/arch/arm/src/stm32/chip/stm32_eth.h index 431144009e..0b5ef18ca7 100644 --- a/nuttx/arch/arm/src/stm32/chip/stm32_eth.h +++ b/nuttx/arch/arm/src/stm32/chip/stm32_eth.h @@ -62,7 +62,9 @@ #define STM32_ETH_MACVLANTR_OFFSET 0x001c /* Ethernet MAC VLAN tag register */ #define STM32_ETH_MACRWUFFR_OFFSET 0x0028 /* Ethernet MAC remote wakeup frame filter reg */ #define STM32_ETH_MACPMTCSR_OFFSET 0x002c /* Ethernet MAC PMT control and status register */ -#define STM32_ETH_MACDBGR_OFFSET 0x0034 /* Ethernet MAC debug register */ +#if defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX) +# define STM32_ETH_MACDBGR_OFFSET 0x0034 /* Ethernet MAC debug register */ +#endif #define STM32_ETH_MACSR_OFFSET 0x0038 /* Ethernet MAC interrupt status register */ #define STM32_ETH_MACIMR_OFFSET 0x003c /* Ethernet MAC interrupt mask register */ #define STM32_ETH_MACA0HR_OFFSET 0x0040 /* Ethernet MAC address 0 high register */ @@ -132,7 +134,9 @@ #define STM32_ETH_MACVLANTR (STM32_ETHERNET_BASE+STM32_ETH_MACVLANTR_OFFSET) #define STM32_ETH_MACRWUFFR (STM32_ETHERNET_BASE+STM32_ETH_MACRWUFFR_OFFSET) #define STM32_ETH_MACPMTCSR (STM32_ETHERNET_BASE+STM32_ETH_MACPMTCSR_OFFSET) -#define STM32_ETH_MACDBGR (STM32_ETHERNET_BASE+STM32_ETH_MACDBGR_OFFSET) +#if defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX) +# define STM32_ETH_MACDBGR (STM32_ETHERNET_BASE+STM32_ETH_MACDBGR_OFFSET) +#endif #define STM32_ETH_MACSR (STM32_ETHERNET_BASE+STM32_ETH_MACSR_OFFSET) #define STM32_ETH_MACIMR (STM32_ETHERNET_BASE+STM32_ETH_MACIMR_OFFSET) #define STM32_ETH_MACA0HR (STM32_ETHERNET_BASE+STM32_ETH_MACA0HR_OFFSET) @@ -216,7 +220,9 @@ # define ETH_MACCR_IFG(n) ((12-((n) >> 3)) << ETH_MACCR_IFG_SHIFT) /* n bit times, n=40,48,..96 */ #define ETH_MACCR_JD (1 << 22) /* Bit 22: Jabber disable */ #define ETH_MACCR_WD (1 << 23) /* Bit 23: Watchdog disable */ -#define ETH_MACCR_CSTF (1 << 25) /* Bits 25: CRC stripping for Type frames */ +#if defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX) +# define ETH_MACCR_CSTF (1 << 25) /* Bits 25: CRC stripping for Type frames */ +#endif /* Ethernet MAC frame filter register */ @@ -303,6 +309,8 @@ /* Ethernet MAC debug register */ +#if defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX) + #define ETH_MACDBGR_MMRPEA (1 << 0) /* Bit 0: MAC MII receive protocol engine active */ #define ETH_MACDBGR_MSFRWCS_SHIFT (1) /* Bits 1-2: MAC small FIFO read / write controllers status */ #define ETH_MACDBGR_MSFRWCS_MASK (3 << ETH_MACDBGR_MSFRWCS_SHIFT) @@ -337,6 +345,8 @@ #define ETH_MACDBGR_TFNE (1 << 24) /* Bit 24: Tx FIFO not empty */ #define ETH_MACDBGR_TFF (1 << 25) /* Bit 25: Tx FIFO full */ +#endif + /* Ethernet MAC interrupt status register */ #define ETH_MACSR_PMTS (1 << 3) /* Bit 3: PMT status */ @@ -419,7 +429,9 @@ #define ETH_MMCCR_ROR (1 << 2) /* Bit 2: Reset on read */ #define ETH_MMCCR_MCF (1 << 3) /* Bit 3: MMC counter freeze */ #define ETH_MMCCR_MCP (1 << 4) /* Bit 4: MMC counter preset */ -#define ETH_MMCCR_MCFHP (1 << 5) /* Bit 5: MMC counter Full-Half preset */ +#if defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX) +# define ETH_MMCCR_MCFHP (1 << 5) /* Bit 5: MMC counter Full-Half preset */ +#endif /* Ethernet MMC receive interrupt and interrupt mask registers */ @@ -453,6 +465,8 @@ #define ETH_PTPTSCR_TSSTU (1 << 3) /* Bit 3: Time stamp system time update */ #define ETH_PTPTSCR_TSITE (1 << 4) /* Bit 4: Time stamp interrupt trigger enable */ #define ETH_PTPTSCR_TSARU (1 << 5) /* Bit 5: Time stamp addend register update */ + +#if defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX) #define ETH_PTPTSCR_TSSARFE (1 << 8) /* Bit 8: Time stamp snapshot for all received frames enable */ #define ETH_PTPTSCR_TSSSR (1 << 9) /* Bit 9: Time stamp subsecond rollover: digital or binary rollover control */ #define ETH_PTPTSCR_TSPTPPSV2E (1 << 10) /* Bit 10: Time stamp PTP packet snooping for version2 format enable */ @@ -468,6 +482,7 @@ # define ETH_PTPTSCR_TSCNT_E2E (2 << ETH_PTPTSCR_TSCNT_SHIFT) /* 10: End-to-end transparent clock */ # define ETH_PTPTSCR_TSCNT_P2P (3 << ETH_PTPTSCR_TSCNT_SHIFT) /* 11: Peer-to-peer transparent clock */ #define ETH_PTPTSCR_TSPFFMAE (1 << 18) /* Bit 18: Time stamp PTP frame filtering MAC address enable */ +#endif /* Ethernet PTP subsecond increment register */ @@ -543,7 +558,9 @@ #define ETH_DMABMR_USP (1 << 23) /* Bit 23: Use separate PBL */ #define ETH_DMABMR_FPM (1 << 24) /* Bit 24: 4xPBL mode */ #define ETH_DMABMR_AAB (1 << 25) /* Bit 25: Address-aligned beats */ -#define ETH_DMABMR_MB (1 << 26) /* Bit 26: Mixed burst */ +#if defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX) +# define ETH_DMABMR_MB (1 << 26) /* Bit 26: Mixed burst */ +#endif /* Ethernet DMA transmit poll demand register (32-bit) */ /* Ethernet DMA receive poll demand register (32-bit) */ diff --git a/nuttx/arch/arm/src/stm32/stm32_eth.c b/nuttx/arch/arm/src/stm32/stm32_eth.c index b3392fa7a4..ab9cac209a 100644 --- a/nuttx/arch/arm/src/stm32/stm32_eth.c +++ b/nuttx/arch/arm/src/stm32/stm32_eth.c @@ -283,14 +283,22 @@ * ETH_MACCR_IFG Bits 17-19: Interframe gap * ETH_MACCR_JD Bit 22: Jabber disable * ETH_MACCR_WD Bit 23: Watchdog disable - * ETH_MACCR_CSTF Bits 25: CRC stripping for Type frames + * ETH_MACCR_CSTF Bits 25: CRC stripping for Type frames (F2/F4 only) */ +#if defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX) #define MACCR_CLEAR_BITS \ - ( ETH_MACCR_RE | ETH_MACCR_TE | ETH_MACCR_DC | ETH_MACCR_BL_MASK | \ + (ETH_MACCR_RE | ETH_MACCR_TE | ETH_MACCR_DC | ETH_MACCR_BL_MASK | \ ETH_MACCR_APCS | ETH_MACCR_RD | ETH_MACCR_IPCO | ETH_MACCR_DM | \ ETH_MACCR_LM | ETH_MACCR_ROD | ETH_MACCR_FES | ETH_MACCR_CSD | \ - ETH_MACCR_IFG_MASK | ETH_MACCR_JD | ETH_MACCR_WD | ETH_MACCR_CSTF ) + ETH_MACCR_IFG_MASK | ETH_MACCR_JD | ETH_MACCR_WD | ETH_MACCR_CSTF) +#else +#define MACCR_CLEAR_BITS \ + (ETH_MACCR_RE | ETH_MACCR_TE | ETH_MACCR_DC | ETH_MACCR_BL_MASK | \ + ETH_MACCR_APCS | ETH_MACCR_RD | ETH_MACCR_IPCO | ETH_MACCR_DM | \ + ETH_MACCR_LM | ETH_MACCR_ROD | ETH_MACCR_FES | ETH_MACCR_CSD | \ + ETH_MACCR_IFG_MASK | ETH_MACCR_JD | ETH_MACCR_WD) +#endif /* The following bits are set or left zero unconditionally in all modes. * @@ -307,7 +315,7 @@ * ETH_MACCR_IFG Interframe gap 0 (96 bits) * ETH_MACCR_JD Jabber disable 0 (enabled) * ETH_MACCR_WD Watchdog disable 0 (enabled) - * ETH_MACCR_CSTF CRC stripping for Type frames 0 (disabled) + * ETH_MACCR_CSTF CRC stripping for Type frames 0 (disabled, F2/F4 only) * * The following are set conditioinally based on mode and speed. * @@ -462,13 +470,20 @@ * ETH_DMABMR_USP Bit 23: Use separate PBL * ETH_DMABMR_FPM Bit 24: 4xPBL mode * ETH_DMABMR_AAB Bit 25: Address-aligned beats - * ETH_DMABMR_MB Bit 26: Mixed burst + * ETH_DMABMR_MB Bit 26: Mixed burst (F2/F4 only) */ +#if defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX) #define DMABMR_CLEAR_MASK \ (ETH_DMABMR_SR | ETH_DMABMR_DA | ETH_DMABMR_DSL_MASK | ETH_DMABMR_EDFE | \ ETH_DMABMR_PBL_MASK | ETH_DMABMR_RTPR_MASK | ETH_DMABMR_FB | ETH_DMABMR_RDP_MASK | \ ETH_DMABMR_USP | ETH_DMABMR_FPM | ETH_DMABMR_AAB | ETH_DMABMR_MB) +#else +#define DMABMR_CLEAR_MASK \ + (ETH_DMABMR_SR | ETH_DMABMR_DA | ETH_DMABMR_DSL_MASK | ETH_DMABMR_EDFE | \ + ETH_DMABMR_PBL_MASK | ETH_DMABMR_RTPR_MASK | ETH_DMABMR_FB | ETH_DMABMR_RDP_MASK | \ + ETH_DMABMR_USP | ETH_DMABMR_FPM | ETH_DMABMR_AAB) +#endif /* The following bits are set or left zero unconditionally in all modes. * @@ -484,7 +499,7 @@ * ETH_DMABMR_USP Use separate PBL 1 (enabled) * ETH_DMABMR_FPM 4xPBL mode 0 (disabled) * ETH_DMABMR_AAB Address-aligned beats 1 (enabled) - * ETH_DMABMR_MB Mixed burst 0 (disabled) + * ETH_DMABMR_MB Mixed burst 0 (disabled, F2/F4 only) */ #ifdef CONFIG_STM32_ETH_ENHANCEDDESC @@ -2001,7 +2016,7 @@ static int stm32_ifup(struct uip_driver_s *dev) ndbg("Bringing up: %d.%d.%d.%d\n", dev->d_ipaddr & 0xff, (dev->d_ipaddr >> 8) & 0xff, - (dev->d_ipaddr >> 16) & 0xff, dev->d_ipaddr >> 24 ); + (dev->d_ipaddr >> 16) & 0xff, dev->d_ipaddr >> 24); /* Configure the Ethernet interface for DMA operation. */ @@ -2242,7 +2257,7 @@ static void stm32_txdescinit(FAR struct stm32_ethmac_s *priv) /* Initialize the next descriptor with the Next Descriptor Polling Enable */ - if( i < (CONFIG_STM32_ETH_NTXDESC-1)) + if (i < (CONFIG_STM32_ETH_NTXDESC-1)) { /* Set next descriptor address register with next descriptor base * address @@ -2321,7 +2336,7 @@ static void stm32_rxdescinit(FAR struct stm32_ethmac_s *priv) /* Initialize the next descriptor with the Next Descriptor Polling Enable */ - if( i < (CONFIG_STM32_ETH_NRXDESC-1)) + if (i < (CONFIG_STM32_ETH_NRXDESC-1)) { /* Set next descriptor address register with next descriptor base * address @@ -2524,7 +2539,7 @@ static int stm32_phyinit(FAR struct stm32_ethmac_s *priv) if (timeout >= PHY_RETRY_TIMEOUT) { - ndbg("Timed out waiting for link status\n"); + ndbg("Timed out waiting for link status: %04x\n", phyval); return -ETIMEDOUT; } diff --git a/nuttx/configs/shenzhou/nsh/defconfig b/nuttx/configs/shenzhou/nsh/defconfig index c4817c2ee0..313f9f57be 100644 --- a/nuttx/configs/shenzhou/nsh/defconfig +++ b/nuttx/configs/shenzhou/nsh/defconfig @@ -172,7 +172,7 @@ CONFIG_STM32_JTAG_FULL_ENABLE=y # # Ethernet MAC configuration # -CONFIG_STM32_PHYADDR=1 +CONFIG_STM32_PHYADDR=0 # CONFIG_STM32_MII is not set CONFIG_STM32_AUTONEG=y CONFIG_STM32_PHYSR=17 diff --git a/nuttx/drivers/net/enc28j60.h b/nuttx/drivers/net/enc28j60.h index 6f7f8f4652..6a0553a95d 100644 --- a/nuttx/drivers/net/enc28j60.h +++ b/nuttx/drivers/net/enc28j60.h @@ -78,7 +78,7 @@ * * The last five locations (0x1b to 0x1f) of all banks point to a common set * of registers: EIE, EIR, ESTAT, ECON2 and ECON1. These are key registers - * usedin controlling and monitoring the operation of the device. Their + * used in controlling and monitoring the operation of the device. Their * common mapping allows easy access without switching the bank. * * Control registers for the ENC28J60 are generically grouped as ETH, MAC and From e2b2cc34504dfb36ca9d06065ed7046efbfc7d78 Mon Sep 17 00:00:00 2001 From: patacongo Date: Mon, 24 Sep 2012 15:51:48 +0000 Subject: [PATCH 28/56] Fixes STM32F107 DMA issue git-svn-id: http://svn.code.sf.net/p/nuttx/code/trunk@5182 42af7a65-404d-4744-a932-0658087f49c3 --- nuttx/ChangeLog | 4 ++++ nuttx/arch/arm/src/stm32/chip/stm32_eth.h | 7 ++++-- nuttx/arch/arm/src/stm32/stm32_eth.c | 16 +++++++++---- nuttx/arch/arm/src/stm32/stm32_idle.c | 23 ++++++++++++++++++- nuttx/configs/fire-stm32v2/nsh/defconfig | 4 ++-- nuttx/configs/shenzhou/nsh/defconfig | 4 ++-- .../configs/stm3240g-eval/discover/defconfig | 4 ++-- nuttx/configs/stm3240g-eval/xmlrpc/defconfig | 4 ++-- 8 files changed, 50 insertions(+), 16 deletions(-) diff --git a/nuttx/ChangeLog b/nuttx/ChangeLog index 42110fd794..eb9680aefa 100644 --- a/nuttx/ChangeLog +++ b/nuttx/ChangeLog @@ -3386,3 +3386,7 @@ case now (either space or a fieldwidth must be provided). But at least some of the bad logic that attempted to handle this case has been removed (noted by Kate). + * arch/arm/src/stm32/stm32_eth.c: DMA buffer sizes must be an + even multiple of 4, 8, or 16 bytes. + * arch/arm/src/stm32/stm32_idle.c: Fixes STM32F107 DMA issues: + We cannot go into sleep mode while Ethernet is actively DMAing. diff --git a/nuttx/arch/arm/src/stm32/chip/stm32_eth.h b/nuttx/arch/arm/src/stm32/chip/stm32_eth.h index 0b5ef18ca7..a4a109d016 100644 --- a/nuttx/arch/arm/src/stm32/chip/stm32_eth.h +++ b/nuttx/arch/arm/src/stm32/chip/stm32_eth.h @@ -711,7 +711,9 @@ /* RDES0: Receive descriptor Word0 */ #define ETH_RDES0_PCE (1 << 0) /* Bit 0: Payload checksum error */ -#define ETH_RDES0_ESA (1 << 0) /* Bit 0: Extended status available */ +#if defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX) +# define ETH_RDES0_ESA (1 << 0) /* Bit 0: Extended status available */ +#endif #define ETH_RDES0_CE (1 << 1) /* Bit 1: CRC error */ #define ETH_RDES0_DBE (1 << 2) /* Bit 2: Dribble bit error */ #define ETH_RDES0_RE (1 << 3) /* Bit 3: Receive error */ @@ -735,8 +737,9 @@ /* RDES1: Receive descriptor Word1 */ -#define ETH_RDES1_RBS1_SHIFT (0) /* Bits 0-12: Receive buffer 1 size */ +#define ETH_RDES1_RBS1_SHIFT (0) /* Bits 0-12: Receive buffer 1 size */ #define ETH_RDES1_RBS1_MASK (0x1fff << ETH_RDES1_RBS1_SHIFT) + /* Bit 13: Reserved */ #define ETH_RDES1_RCH (1 << 14) /* Bit 14: Second address chained */ #define ETH_RDES1_RER (1 << 15) /* Bit 15: Receive end of ring */ #define ETH_RDES1_RBS2_SHIFT (16) /* Bits 16-28: Receive buffer 2 size */ diff --git a/nuttx/arch/arm/src/stm32/stm32_eth.c b/nuttx/arch/arm/src/stm32/stm32_eth.c index ab9cac209a..2e892c9e54 100644 --- a/nuttx/arch/arm/src/stm32/stm32_eth.c +++ b/nuttx/arch/arm/src/stm32/stm32_eth.c @@ -186,10 +186,12 @@ #endif /* Add 4 to the configured buffer size to account for the 2 byte checksum - * memory needed at the end of the maximum size packet. + * memory needed at the end of the maximum size packet. Buffer sizes must + * be an even multiple of 4, 8, or 16 bytes (depending on buswidth). We + * will use the 16-byte alignment in all cases. */ -#define OPTIMAL_ETH_BUFSIZE (CONFIG_NET_BUFSIZE+4) +#define OPTIMAL_ETH_BUFSIZE ((CONFIG_NET_BUFSIZE + 4 + 15) & ~15) #ifndef CONFIG_STM32_ETH_BUFSIZE # define CONFIG_STM32_ETH_BUFSIZE OPTIMAL_ETH_BUFSIZE @@ -199,6 +201,10 @@ # error "CONFIG_STM32_ETH_BUFSIZE is too large" #endif +#if (CONFIG_STM32_ETH_BUFSIZE & 15) != 0 +# error "CONFIG_STM32_ETH_BUFSIZE must be aligned" +#endif + #if CONFIG_STM32_ETH_BUFSIZE != OPTIMAL_ETH_BUFSIZE # warning "You using an incomplete/untested configuration" #endif @@ -1470,9 +1476,6 @@ static int stm32_recvframe(FAR struct stm32_ethmac_s *priv) { priv->segments++; - nllvdbg("rxhead: %p rxcurr: %p segments: %d\n", - priv->rxhead, priv->rxcurr, priv->segments); - /* Check if the there is only one segment in the frame */ if (priv->segments == 1) @@ -1484,6 +1487,9 @@ static int stm32_recvframe(FAR struct stm32_ethmac_s *priv) rxcurr = priv->rxcurr; } + nllvdbg("rxhead: %p rxcurr: %p segments: %d\n", + priv->rxhead, priv->rxcurr, priv->segments); + /* Check if any errors are reported in the frame */ if ((rxdesc->rdes0 & ETH_RDES0_ES) == 0) diff --git a/nuttx/arch/arm/src/stm32/stm32_idle.c b/nuttx/arch/arm/src/stm32/stm32_idle.c index 791a79429c..0b69ff6485 100644 --- a/nuttx/arch/arm/src/stm32/stm32_idle.c +++ b/nuttx/arch/arm/src/stm32/stm32_idle.c @@ -45,6 +45,7 @@ #include +#include "chip.h" #include "stm32_pm.h" #include "up_internal.h" @@ -178,11 +179,31 @@ void up_idle(void) up_idlepm(); - /* Sleep until an interrupt occurs to save power */ + /* Sleep until an interrupt occurs to save power. + * + * NOTE: There is an STM32F107 errata that is fixed by the following + * workaround: + * + * "2.17.11 Ethernet DMA not working after WFI/WFE instruction + * Description + * If a WFI/WFE instruction is executed to put the system in sleep mode + * while the Ethernet MAC master clock on the AHB bus matrix is ON and all + * remaining masters clocks are OFF, the Ethernet DMA will be not able to + * perform any AHB master accesses during sleep mode." + * + * Workaround + * Enable DMA1 or DMA2 clocks in the RCC_AHBENR register before + * executing the WFI/WFE instruction." + * + * Here the workaround is just don't go into SLEEP mode for the connectivity + * line parts if Ethernet is enabled. + */ +#if !defined(CONFIG_STM32_CONNECTIVITYLINE) || !defined(CONFIG_STM32_ETHMAC) BEGIN_IDLE(); asm("WFI"); END_IDLE(); #endif +#endif } diff --git a/nuttx/configs/fire-stm32v2/nsh/defconfig b/nuttx/configs/fire-stm32v2/nsh/defconfig index 0802af3c94..d6aaae05a2 100644 --- a/nuttx/configs/fire-stm32v2/nsh/defconfig +++ b/nuttx/configs/fire-stm32v2/nsh/defconfig @@ -456,7 +456,7 @@ CONFIG_NET_BUFSIZE=562 CONFIG_NET_TCP=y CONFIG_NET_TCP_CONNS=16 CONFIG_NET_MAX_LISTENPORTS=16 -CONFIG_NET_TCP_READAHEAD_BUFSIZE=562 +# CONFIG_NET_TCP_READAHEAD_BUFSIZE=562 CONFIG_NET_NTCP_READAHEAD_BUFFERS=8 CONFIG_NET_TCP_RECVDELAY=0 # CONFIG_NET_TCPBACKLOG is not set @@ -469,7 +469,7 @@ CONFIG_NET_ICMP_PING=y # CONFIG_NET_PINGADDRCONF is not set # CONFIG_NET_IGMP is not set CONFIG_NET_STATISTICS=y -CONFIG_NET_RECEIVE_WINDOW=562 +# CONFIG_NET_RECEIVE_WINDOW=562 CONFIG_NET_ARPTAB_SIZE=16 # CONFIG_NET_ARP_IPIN is not set diff --git a/nuttx/configs/shenzhou/nsh/defconfig b/nuttx/configs/shenzhou/nsh/defconfig index 313f9f57be..0a699d334d 100644 --- a/nuttx/configs/shenzhou/nsh/defconfig +++ b/nuttx/configs/shenzhou/nsh/defconfig @@ -388,7 +388,7 @@ CONFIG_NET_BUFSIZE=562 CONFIG_NET_TCP=y CONFIG_NET_TCP_CONNS=40 CONFIG_NET_MAX_LISTENPORTS=40 -CONFIG_NET_TCP_READAHEAD_BUFSIZE=562 +# CONFIG_NET_TCP_READAHEAD_BUFSIZE=562 CONFIG_NET_NTCP_READAHEAD_BUFFERS=16 CONFIG_NET_TCP_RECVDELAY=0 CONFIG_NET_TCPBACKLOG=y @@ -401,7 +401,7 @@ CONFIG_NET_ICMP_PING=y # CONFIG_NET_PINGADDRCONF is not set # CONFIG_NET_IGMP is not set CONFIG_NET_STATISTICS=y -CONFIG_NET_RECEIVE_WINDOW=562 +# CONFIG_NET_RECEIVE_WINDOW=562 CONFIG_NET_ARPTAB_SIZE=16 # CONFIG_NET_ARP_IPIN is not set diff --git a/nuttx/configs/stm3240g-eval/discover/defconfig b/nuttx/configs/stm3240g-eval/discover/defconfig index 81a5b3c910..71dce1028b 100644 --- a/nuttx/configs/stm3240g-eval/discover/defconfig +++ b/nuttx/configs/stm3240g-eval/discover/defconfig @@ -388,7 +388,7 @@ CONFIG_NET_BUFSIZE=650 CONFIG_NET_TCP=y CONFIG_NET_TCP_CONNS=40 CONFIG_NET_MAX_LISTENPORTS=40 -CONFIG_NET_TCP_READAHEAD_BUFSIZE=562 +# CONFIG_NET_TCP_READAHEAD_BUFSIZE=562 CONFIG_NET_NTCP_READAHEAD_BUFFERS=16 CONFIG_NET_TCP_RECVDELAY=0 CONFIG_NET_TCPBACKLOG=y @@ -401,7 +401,7 @@ CONFIG_NET_ICMP_PING=y # CONFIG_NET_PINGADDRCONF is not set # CONFIG_NET_IGMP is not set CONFIG_NET_STATISTICS=y -CONFIG_NET_RECEIVE_WINDOW=562 +# CONFIG_NET_RECEIVE_WINDOW=562 CONFIG_NET_ARPTAB_SIZE=16 CONFIG_NET_ARP_IPIN=y diff --git a/nuttx/configs/stm3240g-eval/xmlrpc/defconfig b/nuttx/configs/stm3240g-eval/xmlrpc/defconfig index 5c70327b9c..cd80ea618d 100644 --- a/nuttx/configs/stm3240g-eval/xmlrpc/defconfig +++ b/nuttx/configs/stm3240g-eval/xmlrpc/defconfig @@ -382,7 +382,7 @@ CONFIG_NET_BUFSIZE=650 CONFIG_NET_TCP=y CONFIG_NET_TCP_CONNS=40 CONFIG_NET_MAX_LISTENPORTS=40 -CONFIG_NET_TCP_READAHEAD_BUFSIZE=562 +# CONFIG_NET_TCP_READAHEAD_BUFSIZE=562 CONFIG_NET_NTCP_READAHEAD_BUFFERS=16 CONFIG_NET_TCP_RECVDELAY=0 CONFIG_NET_TCPBACKLOG=y @@ -395,7 +395,7 @@ CONFIG_NET_ICMP_PING=y # CONFIG_NET_PINGADDRCONF is not set # CONFIG_NET_IGMP is not set CONFIG_NET_STATISTICS=y -CONFIG_NET_RECEIVE_WINDOW=562 +# CONFIG_NET_RECEIVE_WINDOW=562 CONFIG_NET_ARPTAB_SIZE=16 CONFIG_NET_ARP_IPIN=y From cafa22293a223e129d6d5ea3bc081fe07e1313be Mon Sep 17 00:00:00 2001 From: patacongo Date: Mon, 24 Sep 2012 18:57:38 +0000 Subject: [PATCH 29/56] Corrections for SD card on Shenzhou board git-svn-id: http://svn.code.sf.net/p/nuttx/code/trunk@5183 42af7a65-404d-4744-a932-0658087f49c3 --- nuttx/Documentation/NuttX.html | 13 +++++++++---- nuttx/arch/arm/src/stm32/stm32_idle.c | 6 ++++-- nuttx/configs/shenzhou/README.txt | 13 +++++++++++-- nuttx/configs/shenzhou/nsh/defconfig | 10 +++++----- .../configs/shenzhou/src/shenzhou-internal.h | 2 +- nuttx/configs/shenzhou/src/up_mmcsd.c | 2 +- nuttx/configs/shenzhou/src/up_nsh.c | 4 ++-- nuttx/configs/shenzhou/src/up_spi.c | 19 ++++++++++--------- nuttx/drivers/Kconfig | 2 +- 9 files changed, 44 insertions(+), 27 deletions(-) diff --git a/nuttx/Documentation/NuttX.html b/nuttx/Documentation/NuttX.html index b51b5e7b32..84e0024ef8 100644 --- a/nuttx/Documentation/NuttX.html +++ b/nuttx/Documentation/NuttX.html @@ -1789,7 +1789,7 @@ Not all features have been verified. Support for FAT file system on an an SD card had been verified. The ENC28J60 network is functional (but required lifting the chip select pin on the W25x16 part). - Customizations for the v3 version of the Wildfire board a selectable (but untested). + Customizations for the v3 version of the Wildfire board are selectable (but untested).

    @@ -1820,15 +1820,20 @@

      STATUS: Configurations for the basic OS test and NSH are available and verified. + Networking is functional.

    - Shenzhou - Work is underway as of this writing to port NuttX to the Shenzhou development board (See www.armjishu.com) featuring the STMicro STM32F107VCT MCU. - If all goes according to plan, this port should be verified and avaialble in NuttX-6.22. + Shenzhou IV + Work is underway as of this writing to port NuttX to the Shenzhou IV development board (See www.armjishu.com) featuring the STMicro STM32F107VCT MCU. + If all goes according to plan, this port should be verified and available in NuttX-6.22.

      STATUS: In progress. + The following have been verified: + (1) Basic Cortex-M3 port, + (2) Ethernet, + (3) On-board LEDs

    diff --git a/nuttx/arch/arm/src/stm32/stm32_idle.c b/nuttx/arch/arm/src/stm32/stm32_idle.c index 0b69ff6485..83a6808c58 100644 --- a/nuttx/arch/arm/src/stm32/stm32_idle.c +++ b/nuttx/arch/arm/src/stm32/stm32_idle.c @@ -195,8 +195,10 @@ void up_idle(void) * Enable DMA1 or DMA2 clocks in the RCC_AHBENR register before * executing the WFI/WFE instruction." * - * Here the workaround is just don't go into SLEEP mode for the connectivity - * line parts if Ethernet is enabled. + * Here the workaround is just to avoid SLEEP mode for the connectivity + * line parts if Ethernet is enabled. The errate recommends a more + * general solution: Enabling DMA1/2 clocking in stm32f10xx_rcc.c if the + * STM32107 Ethernet peripheral is enabled. */ #if !defined(CONFIG_STM32_CONNECTIVITYLINE) || !defined(CONFIG_STM32_ETHMAC) diff --git a/nuttx/configs/shenzhou/README.txt b/nuttx/configs/shenzhou/README.txt index e929f2b8ba..23535e8d9c 100644 --- a/nuttx/configs/shenzhou/README.txt +++ b/nuttx/configs/shenzhou/README.txt @@ -2,8 +2,17 @@ README ====== This README discusses issues unique to NuttX configurations for the Shenzhou -development board from www.armjishu.com featuring the STMicro STM32F107VCT -MCU. On-board features: +IV development board from www.armjishu.com featuring the STMicro STM32F107VCT +MCU. As of this writing, there are five models of the Shenzhou board: + + 1. Shenzhou I (STM32F103RB) + 2. Shenzhou II (STM32F103VC) + 3. Shenzhou III (STM32F103ZE) + 4. Shenzhou IV (STM32F107VC) + 5. Shenzhou king ((STM32F103ZG, core board + IO expansion board)). + +Support is currently provided for the Shenzhou IV only. Features of the +Shenzhou IV board include: - STM32F107VCT - 10/100M PHY (DM9161AEP) diff --git a/nuttx/configs/shenzhou/nsh/defconfig b/nuttx/configs/shenzhou/nsh/defconfig index 0a699d334d..3528f6d605 100644 --- a/nuttx/configs/shenzhou/nsh/defconfig +++ b/nuttx/configs/shenzhou/nsh/defconfig @@ -233,7 +233,7 @@ CONFIG_ARCH_HAVE_BUTTONS=y CONFIG_ARCH_HAVE_IRQBUTTONS=y CONFIG_NSH_MMCSDMINOR=0 CONFIG_NSH_MMCSDSLOTNO=0 -CONFIG_NSH_MMCSDSPIPORTNO=0 +CONFIG_NSH_MMCSDSPIPORTNO=1 # # Board-Specific Options @@ -315,7 +315,7 @@ CONFIG_ARCH_HAVE_I2CRESET=y CONFIG_SPI=y # CONFIG_SPI_OWNBUS is not set CONFIG_SPI_EXCHANGE=y -CONFIG_SPI_CMDDATA=y +# CONFIG_SPI_CMDDATA is not set CONFIG_RTC=y # CONFIG_RTC_DATETIME is not set # CONFIG_RTC_HIRES is not set @@ -388,7 +388,7 @@ CONFIG_NET_BUFSIZE=562 CONFIG_NET_TCP=y CONFIG_NET_TCP_CONNS=40 CONFIG_NET_MAX_LISTENPORTS=40 -# CONFIG_NET_TCP_READAHEAD_BUFSIZE=562 +CONFIG_NET_TCP_READAHEAD_BUFSIZE=562 CONFIG_NET_NTCP_READAHEAD_BUFFERS=16 CONFIG_NET_TCP_RECVDELAY=0 CONFIG_NET_TCPBACKLOG=y @@ -401,7 +401,7 @@ CONFIG_NET_ICMP_PING=y # CONFIG_NET_PINGADDRCONF is not set # CONFIG_NET_IGMP is not set CONFIG_NET_STATISTICS=y -# CONFIG_NET_RECEIVE_WINDOW=562 +CONFIG_NET_RECEIVE_WINDOW=562 CONFIG_NET_ARPTAB_SIZE=16 # CONFIG_NET_ARP_IPIN is not set @@ -877,7 +877,7 @@ CONFIG_NSH_NESTDEPTH=3 # CONFIG_NSH_DISABLEBG is not set CONFIG_NSH_CONSOLE=y # CONFIG_NSH_CONDEV is not set -# CONFIG_NSH_ARCHINIT is not set +CONFIG_NSH_ARCHINIT=y CONFIG_NSH_TELNET=y CONFIG_NSH_TELNETD_PORT=23 CONFIG_NSH_TELNETD_DAEMONPRIO=100 diff --git a/nuttx/configs/shenzhou/src/shenzhou-internal.h b/nuttx/configs/shenzhou/src/shenzhou-internal.h index b5fb4e35c0..0dc3b067e6 100644 --- a/nuttx/configs/shenzhou/src/shenzhou-internal.h +++ b/nuttx/configs/shenzhou/src/shenzhou-internal.h @@ -230,7 +230,7 @@ * PN NAME SIGNAL NOTES * -- ---- -------------- ------------------------------------------------------------------- * 53 PB14 SD_CD Active low: Pulled high - * 58 PD11 SD_CS + * 58 PD11 SD_CS Active low: Pulled high */ #define GPIO_SD_CD (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN14) diff --git a/nuttx/configs/shenzhou/src/up_mmcsd.c b/nuttx/configs/shenzhou/src/up_mmcsd.c index daa1498178..531fbfd7e3 100644 --- a/nuttx/configs/shenzhou/src/up_mmcsd.c +++ b/nuttx/configs/shenzhou/src/up_mmcsd.c @@ -67,7 +67,7 @@ /* Can't support MMC/SD features if mountpoints are disabled */ -#ifndef CONFIG_DISABLE_MOUNTPOINT +#ifdef CONFIG_DISABLE_MOUNTPOINT # undef HAVE_MMCSD #endif diff --git a/nuttx/configs/shenzhou/src/up_nsh.c b/nuttx/configs/shenzhou/src/up_nsh.c index e6126ba4d2..685281f5da 100644 --- a/nuttx/configs/shenzhou/src/up_nsh.c +++ b/nuttx/configs/shenzhou/src/up_nsh.c @@ -116,7 +116,7 @@ /* Can't support W25 features if mountpoints are disabled */ -#if defined(CONFIG_DISABLE_MOUNTPOINT) +#ifdef CONFIG_DISABLE_MOUNTPOINT # undef HAVE_W25 #endif @@ -194,7 +194,7 @@ int nsh_archinitialize(void) /* Initialize the SPI-based MMC/SD slot */ #ifdef HAVE_MMCSD - ret = stm32_sdinitialze(CONFIG_NSH_MMCSDMINOR); + ret = stm32_sdinitialize(CONFIG_NSH_MMCSDMINOR); if (ret < 0) { message("nsh_archinitialize: Failed to initialize MMC/SD slot %d: %d\n", diff --git a/nuttx/configs/shenzhou/src/up_spi.c b/nuttx/configs/shenzhou/src/up_spi.c index ce6a1f75e9..8f74c92f78 100644 --- a/nuttx/configs/shenzhou/src/up_spi.c +++ b/nuttx/configs/shenzhou/src/up_spi.c @@ -60,18 +60,18 @@ /* Enables debug output from this file (needs CONFIG_DEBUG too) */ -#undef SPI_DEBUG /* Define to enable debug */ -#undef SPI_VERBOSE /* Define to enable verbose debug */ +#ifndef CONFIG_DEBUG +# undef CONFIG_DEBUG_SPI +#endif -#ifdef SPI_DEBUG +#ifdef CONFIG_DEBUG_SPI # define spidbg lldbg -# ifdef SPI_VERBOSE +# ifdef CONFIG_DEBUG_VERBOSE # define spivdbg lldbg # else # define spivdbg(x...) # endif #else -# undef SPI_VERBOSE # define spidbg(x...) # define spivdbg(x...) #endif @@ -103,15 +103,16 @@ void weak_function stm32_spiinitialize(void) /* SPI1 connects to the SD CARD and to the SPI FLASH */ #ifdef CONFIG_STM32_SPI1 - stm32_configgpio(GPIO_SD_CS); - stm32_configgpio(GPIO_FLASH_CS); + stm32_configgpio(GPIO_SD_CS); /* SD card chip select */ + stm32_configgpio(GPIO_SD_CD); /* SD card detect */ + stm32_configgpio(GPIO_FLASH_CS); /* FLASH chip select */ #endif /* SPI3 connects to TFT LCD and the RF24L01 2.4G wireless module */ #ifdef CONFIG_STM32_SPI3 - stm32_configgpio(GPIO_LCD_CS); - stm32_configgpio(GPIO_WIRELESS_CS); + stm32_configgpio(GPIO_LCD_CS); /* LCD chip select */ + stm32_configgpio(GPIO_WIRELESS_CS); /* Wireless chip select */ #endif } diff --git a/nuttx/drivers/Kconfig b/nuttx/drivers/Kconfig index 32d127aa43..ea218a5925 100644 --- a/nuttx/drivers/Kconfig +++ b/nuttx/drivers/Kconfig @@ -151,7 +151,7 @@ config SPI_EXCHANGE config SPI_CMDDATA bool "SPI CMD/DATA" - default y + default n ---help--- Devices on the SPI bus require out-of-band support to distinguish command transfers from data transfers. Such devices will often support either 9-bit From 169c6208456cbad9c85a42d1a9cf0703c02370ee Mon Sep 17 00:00:00 2001 From: patacongo Date: Mon, 24 Sep 2012 19:37:56 +0000 Subject: [PATCH 30/56] Add framework for SSD1289 LCD on Shenzhou git-svn-id: http://svn.code.sf.net/p/nuttx/code/trunk@5184 42af7a65-404d-4744-a932-0658087f49c3 --- nuttx/ChangeLog | 5 + nuttx/configs/shenzhou/README.txt | 2 +- nuttx/configs/shenzhou/src/Makefile | 4 + .../configs/shenzhou/src/shenzhou-internal.h | 57 +-- nuttx/configs/shenzhou/src/up_ssd1289.c | 425 ++++++++++++++++++ 5 files changed, 467 insertions(+), 26 deletions(-) create mode 100644 nuttx/configs/shenzhou/src/up_ssd1289.c diff --git a/nuttx/ChangeLog b/nuttx/ChangeLog index eb9680aefa..b7f1bc949d 100644 --- a/nuttx/ChangeLog +++ b/nuttx/ChangeLog @@ -3390,3 +3390,8 @@ even multiple of 4, 8, or 16 bytes. * arch/arm/src/stm32/stm32_idle.c: Fixes STM32F107 DMA issues: We cannot go into sleep mode while Ethernet is actively DMAing. + * configs/shenzhou/src/up_ssd1289.c: Add infrastructure to support + SSD1289 LCD. Initial checkin is just a clone of the + STM32F4Discovery's FSMC-based LCD interface. The Shenzhou + will need a completely need bit-banging interface; this + initial check-in is only for the framework. diff --git a/nuttx/configs/shenzhou/README.txt b/nuttx/configs/shenzhou/README.txt index 23535e8d9c..b083f293ec 100644 --- a/nuttx/configs/shenzhou/README.txt +++ b/nuttx/configs/shenzhou/README.txt @@ -136,7 +136,7 @@ PN NAME SIGNAL NOTES RMII_CRSDV Ethernet PHY 56 PD9 MII_RXD0 Ethernet PHY 57 PD10 MII_RXD1 Ethernet PHY -58 PD11 SD_CS Active low: Pulled high +58 PD11 SD_CS Active low: Pulled high (See also TFT LCD CN13, pin 32) 59 PD12 WIRELESS_CS To the NRF24L01 2.4G wireless module 60 PD13 LCD_RS To TFT LCD (CN13) 61 PD14 LCD_WR To TFT LCD (CN13) diff --git a/nuttx/configs/shenzhou/src/Makefile b/nuttx/configs/shenzhou/src/Makefile index c12a251f0f..2bfec8072c 100644 --- a/nuttx/configs/shenzhou/src/Makefile +++ b/nuttx/configs/shenzhou/src/Makefile @@ -84,6 +84,10 @@ ifeq ($(CONFIG_WATCHDOG),y) CSRCS += up_watchdog.c endif +ifeq ($(CONFIG_LCD_SSD1289),y) +CSRCS += up_ssd1289.c +endif + COBJS = $(CSRCS:.c=$(OBJEXT)) SRCS = $(ASRCS) $(CSRCS) diff --git a/nuttx/configs/shenzhou/src/shenzhou-internal.h b/nuttx/configs/shenzhou/src/shenzhou-internal.h index 0dc3b067e6..32df162ba1 100644 --- a/nuttx/configs/shenzhou/src/shenzhou-internal.h +++ b/nuttx/configs/shenzhou/src/shenzhou-internal.h @@ -154,30 +154,37 @@ * -- ---- -------------- ------------------------------------------------------------------- * PN NAME SIGNAL NOTES * -- ---- -------------- ------------------------------------------------------------------- - * 37 PB2 DATA_LE To TFT LCD (CN13) - * 96 PB9 F_CS To both the TFT LCD (CN13) and to the W25X16 SPI FLASH - * 34 PC5 TP_INT JP6. To TFT LCD (CN13) module - * 65 PC8 LCD_CS Active low: Pulled high - * 66 PC9 TP_CS Active low: Pulled high - * 60 PD13 LCD_RS To TFT LCD (CN13) - * 61 PD14 LCD_WR To TFT LCD (CN13) - * 62 PD15 LCD_RD To TFT LCD (CN13) - * 97 PE0 DB00 To TFT LCD (CN13) - * 98 PE1 DB01 To TFT LCD (CN13) - * 1 PE2 DB02 To TFT LCD (CN13) - * 2 PE3 DB03 To TFT LCD (CN13) - * 3 PE4 DB04 To TFT LCD (CN13) - * 4 PE5 DB05 To TFT LCD (CN13) - * 5 PE6 DB06 To TFT LCD (CN13) - * 38 PE7 DB07 To TFT LCD (CN13) - * 39 PE8 DB08 To TFT LCD (CN13) - * 40 PE9 DB09 To TFT LCD (CN13) - * 41 PE10 DB10 To TFT LCD (CN13) - * 42 PE11 DB11 To TFT LCD (CN13) - * 43 PE12 DB12 To TFT LCD (CN13) - * 44 PE13 DB13 To TFT LCD (CN13) - * 45 PE14 DB14 To TFT LCD (CN13) - * 46 PE15 DB15 To TFT LCD (CN13) + * 37 PB2 DATA_LE To TFT LCD (CN13, ping 28) + * 96 PB9 F_CS To both the TFT LCD (CN13, pin 30) and to the W25X16 SPI FLASH + * 34 PC5 TP_INT JP6. To TFT LCD (CN13) module (CN13, pin 26) + * 65 PC8 LCD_CS Active low: Pulled high (CN13, pin 19) + * 66 PC9 TP_CS Active low: Pulled high (CN13, pin 31) + * 78 PC10 SPI3_SCK To TFT LCD (CN13, pin 29) + * 79 PC11 SPI3_MISO To TFT LCD (CN13, pin 25) + * 80 PC12 SPI3_MOSI To TFT LCD (CN13, pin 27) + * 58 PD11 SD_CS Active low: Pulled high (See also TFT LCD CN13, pin 32) + * 60 PD13 LCD_RS To TFT LCD (CN13, pin 20) + * 61 PD14 LCD_WR To TFT LCD (CN13, pin 21) + * 62 PD15 LCD_RD To TFT LCD (CN13, pin 22) + * 97 PE0 DB00 To TFT LCD (CN13, pin 3) + * 98 PE1 DB01 To TFT LCD (CN13, pin 4) + * 1 PE2 DB02 To TFT LCD (CN13, pin 5) + * 2 PE3 DB03 To TFT LCD (CN13, pin 6) + * 3 PE4 DB04 To TFT LCD (CN13, pin 7) + * 4 PE5 DB05 To TFT LCD (CN13, pin 8) + * 5 PE6 DB06 To TFT LCD (CN13, pin 9) + * 38 PE7 DB07 To TFT LCD (CN13, pin 10) + * 39 PE8 DB08 To TFT LCD (CN13, pin 11) + * 40 PE9 DB09 To TFT LCD (CN13, pin 12) + * 41 PE10 DB10 To TFT LCD (CN13, pin 13) + * 42 PE11 DB11 To TFT LCD (CN13, pin 16) + * 43 PE12 DB12 To TFT LCD (CN13, pin 15) + * 44 PE13 DB13 To TFT LCD (CN13, pin 16) + * 45 PE14 DB14 To TFT LCD (CN13, pin 17) + * 46 PE15 DB15 To TFT LCD (CN13, pin 18) + * + * NOTE: The backlight signl NC_BL (CN13, pin 24) is pulled high and not under + * software control */ #define GPIO_LCD_CS (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ @@ -230,7 +237,7 @@ * PN NAME SIGNAL NOTES * -- ---- -------------- ------------------------------------------------------------------- * 53 PB14 SD_CD Active low: Pulled high - * 58 PD11 SD_CS Active low: Pulled high + * 58 PD11 SD_CS Active low: Pulled high (See also TFT LCD CN13, pin 32) */ #define GPIO_SD_CD (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN14) diff --git a/nuttx/configs/shenzhou/src/up_ssd1289.c b/nuttx/configs/shenzhou/src/up_ssd1289.c new file mode 100644 index 0000000000..6c24e54042 --- /dev/null +++ b/nuttx/configs/shenzhou/src/up_ssd1289.c @@ -0,0 +1,425 @@ +/************************************************************************************ + * configs/shenzhou/src/up_ssd1289.c + * arch/arm/src/board/up_ssd1289.c + * + * This logic supports the connection of an SSD1289-based LCD to the Shenzhou IV + * board. + * + * Copyright (C) 2012 Gregory Nutt. All rights reserved. + * Authors: 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 "up_arch.h" +#include "stm32.h" +#include "stm32_internal.h" +#include "shenzhou-internal.h" + +#ifdef CONFIG_LCD_SSD1289 + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ +/* Configuration ********************************************************************/ + +#ifndef CONFIG_STM32_FSMC +# error "CONFIG_STM32_FSMC is required to use the LCD" +#endif + +/* Define CONFIG_DEBUG_LCD to enable detailed LCD debug output. Verbose debug must + * also be enabled. + */ + +#ifndef CONFIG_DEBUG +# undef CONFIG_DEBUG_VERBOSE +# undef CONFIG_DEBUG_GRAPHICS +# undef CONFIG_DEBUG_LCD +#endif + +#ifndef CONFIG_DEBUG_VERBOSE +# undef CONFIG_DEBUG_LCD +#endif + +/* Shenzhou LCD Hardware Definitions ************************************************/ +/* LCD /CS is CE1 == NOR/SRAM Bank 1 + * + * Bank 1 = 0x60000000 | 0x00000000 + * Bank 2 = 0x60000000 | 0x04000000 + * Bank 3 = 0x60000000 | 0x08000000 + * Bank 4 = 0x60000000 | 0x0c000000 + * + * FSMC address bit 16 is used to distinguish command and data. FSMC address bits + * 0-24 correspond to ARM address bits 1-25. + */ + +#define STM32_LCDBASE ((uintptr_t)(0x60000000 | 0x00000000)) +#define LCD_INDEX (STM32_LCDBASE) +#define LCD_DATA (STM32_LCDBASE + 0x00020000) + +/* SRAM pin definitions */ + +#define LCD_NADDRLINES 1 /* A16 */ +#define LCD_NDATALINES 16 /* D0-15 */ + +/* Debug ****************************************************************************/ + +#ifdef CONFIG_DEBUG_LCD +# define lcddbg dbg +# define lcdvdbg vdbg +#else +# define lcddbg(x...) +# define lcdvdbg(x...) +#endif + +/************************************************************************************ + * Private Type Definition + ************************************************************************************/ + +/************************************************************************************** + * Private Function Protototypes + **************************************************************************************/ +/* Low Level LCD access */ + +static void stm32_select(FAR struct ssd1289_lcd_s *dev); +static void stm32_deselect(FAR struct ssd1289_lcd_s *dev); +static void stm32_index(FAR struct ssd1289_lcd_s *dev, uint8_t index); +#ifndef CONFIG_SSD1289_WRONLY +static uint16_t stm32_read(FAR struct ssd1289_lcd_s *dev); +#endif +static void stm32_write(FAR struct ssd1289_lcd_s *dev, uint16_t data); +static void stm32_backlight(FAR struct ssd1289_lcd_s *dev, int power); + +/************************************************************************************ + * Private Data + ************************************************************************************/ +/* TFT LCD + * + * -- ---- -------------- ------------------------------------------------------------------- + * PN NAME SIGNAL NOTES + * -- ---- -------------- ------------------------------------------------------------------- + * 37 PB2 DATA_LE To TFT LCD (CN13, ping 28) + * 96 PB9 F_CS To both the TFT LCD (CN13, pin 30) and to the W25X16 SPI FLASH + * 34 PC5 TP_INT JP6. To TFT LCD (CN13) module (CN13, pin 26) + * 65 PC8 LCD_CS Active low: Pulled high (CN13, pin 19) + * 66 PC9 TP_CS Active low: Pulled high (CN13, pin 31) + * 78 PC10 SPI3_SCK To TFT LCD (CN13, pin 29) + * 79 PC11 SPI3_MISO To TFT LCD (CN13, pin 25) + * 80 PC12 SPI3_MOSI To TFT LCD (CN13, pin 27) + * 58 PD11 SD_CS Active low: Pulled high (See also TFT LCD CN13, pin 32) + * 60 PD13 LCD_RS To TFT LCD (CN13, pin 20) + * 61 PD14 LCD_WR To TFT LCD (CN13, pin 21) + * 62 PD15 LCD_RD To TFT LCD (CN13, pin 22) + * 97 PE0 DB00 To TFT LCD (CN13, pin 3) + * 98 PE1 DB01 To TFT LCD (CN13, pin 4) + * 1 PE2 DB02 To TFT LCD (CN13, pin 5) + * 2 PE3 DB03 To TFT LCD (CN13, pin 6) + * 3 PE4 DB04 To TFT LCD (CN13, pin 7) + * 4 PE5 DB05 To TFT LCD (CN13, pin 8) + * 5 PE6 DB06 To TFT LCD (CN13, pin 9) + * 38 PE7 DB07 To TFT LCD (CN13, pin 10) + * 39 PE8 DB08 To TFT LCD (CN13, pin 11) + * 40 PE9 DB09 To TFT LCD (CN13, pin 12) + * 41 PE10 DB10 To TFT LCD (CN13, pin 13) + * 42 PE11 DB11 To TFT LCD (CN13, pin 16) + * 43 PE12 DB12 To TFT LCD (CN13, pin 15) + * 44 PE13 DB13 To TFT LCD (CN13, pin 16) + * 45 PE14 DB14 To TFT LCD (CN13, pin 17) + * 46 PE15 DB15 To TFT LCD (CN13, pin 18) + * + * NOTE: The backlight signl NC_BL (CN13, pin 24) is pulled high and not under + * software control + */ + +#define GPIO_LCD_RESET (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\ + GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN6) + +/* GPIO configurations unique to the LCD */ + +static const uint32_t g_lcdconfig[] = +{ + /* PC6(RESET), FSMC_A16, FSMC_NOE, FSMC_NWE, and FSMC_NE1 */ + + GPIO_LCD_RESET, GPIO_FSMC_A16, GPIO_FSMC_NOE, GPIO_FSMC_NWE, GPIO_FSMC_NE1 +}; +#define NLCD_CONFIG (sizeof(g_lcdconfig)/sizeof(uint32_t)) + +/* This is the driver state structure (there is no retained state information) */ + +static struct ssd1289_lcd_s g_ssd1289 = +{ + .select = stm32_select, + .deselect = stm32_deselect, + .index = stm32_index, +#ifndef CONFIG_SSD1289_WRONLY + .read = stm32_read, +#endif + .write = stm32_write, + .backlight = stm32_backlight +}; + +/* The saved instance of the LCD driver */ + +static FAR struct lcd_dev_s *g_ssd1289drvr; + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: stm32_select + * + * Description: + * Select the LCD device + * + ************************************************************************************/ + +static void stm32_select(FAR struct ssd1289_lcd_s *dev) +{ + /* Does not apply to this hardware */ +} + +/************************************************************************************ + * Name: stm32_deselect + * + * Description: + * De-select the LCD device + * + ************************************************************************************/ + +static void stm32_deselect(FAR struct ssd1289_lcd_s *dev) +{ + /* Does not apply to this hardware */ +} + +/************************************************************************************ + * Name: stm32_deselect + * + * Description: + * Set the index register + * + ************************************************************************************/ + +static void stm32_index(FAR struct ssd1289_lcd_s *dev, uint8_t index) +{ + putreg16((uint16_t)index, LCD_INDEX); +} + +/************************************************************************************ + * Name: stm32_read + * + * Description: + * Read LCD data (GRAM data or register contents) + * + ************************************************************************************/ + +#ifndef CONFIG_SSD1289_WRONLY +static uint16_t stm32_read(FAR struct ssd1289_lcd_s *dev) +{ + return getreg16(LCD_DATA); +} +#endif + +/************************************************************************************ + * Name: stm32_write + * + * Description: + * Write LCD data (GRAM data or register contents) + * + ************************************************************************************/ + +static void stm32_write(FAR struct ssd1289_lcd_s *dev, uint16_t data) +{ + putreg16((uint16_t)data, LCD_DATA); +} + +/************************************************************************************ + * Name: stm32_write + * + * Description: + * Write LCD data (GRAM data or register contents) + * + ************************************************************************************/ + +static void stm32_backlight(FAR struct ssd1289_lcd_s *dev, int power) +{ +#warning "Missing logic" +} + +/************************************************************************************ + * Name: stm32_selectlcd + * + * Description: + * Initialize to the LCD + * + ************************************************************************************/ + +void stm32_selectlcd(void) +{ + /* Configure GPIO pins */ + + stm32_extmemdata(LCD_NDATALINES); /* Common data lines: D0-D15 */ + stm32_extmemgpios(g_lcdconfig, NLCD_CONFIG); /* LCD-specific control lines */ + + /* Enable AHB clocking to the FSMC */ + + stm32_enablefsmc(); + + /* Color LCD configuration (LCD configured as follow): + * + * - Data/Address MUX = Disable "FSMC_BCR_MUXEN" just not enable it. + * - Extended Mode = Disable "FSMC_BCR_EXTMOD" + * - Memory Type = SRAM "FSMC_BCR_SRAM" + * - Data Width = 16bit "FSMC_BCR_MWID16" + * - Write Operation = Enable "FSMC_BCR_WREN" + * - Asynchronous Wait = Disable + */ + + /* Bank1 NOR/SRAM control register configuration */ + + putreg32(FSMC_BCR_SRAM | FSMC_BCR_MWID16 | FSMC_BCR_WREN, STM32_FSMC_BCR1); + + /* Bank1 NOR/SRAM timing register configuration */ + + putreg32(FSMC_BTR_ADDSET(5) | FSMC_BTR_ADDHLD(0) | FSMC_BTR_DATAST(9) | FSMC_BTR_BUSTRUN(0) | + FSMC_BTR_CLKDIV(0) | FSMC_BTR_DATLAT(0) | FSMC_BTR_ACCMODA, STM32_FSMC_BTR1); + + putreg32(0xffffffff, STM32_FSMC_BWTR1); + + /* Enable the bank by setting the MBKEN bit */ + + putreg32(FSMC_BCR_MBKEN | FSMC_BCR_SRAM | FSMC_BCR_MWID16 | FSMC_BCR_WREN, STM32_FSMC_BCR1); +} + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: up_lcdinitialize + * + * Description: + * Initialize the LCD video hardware. The initial state of the LCD is fully + * initialized, display memory cleared, and the LCD ready to use, but with the power + * setting at 0 (full off). + * + ************************************************************************************/ + +int up_lcdinitialize(void) +{ + /* Only initialize the driver once */ + + if (!g_ssd1289drvr) + { + lcdvdbg("Initializing\n"); + + /* Configure GPIO pins and configure the FSMC to support the LCD */ + + stm32_selectlcd(); + + /* Reset the LCD (active low) */ + + stm32_gpiowrite(GPIO_LCD_RESET, false); + up_mdelay(5); + stm32_gpiowrite(GPIO_LCD_RESET, true); + + /* Configure and enable the LCD */ + + up_mdelay(50); + g_ssd1289drvr = ssd1289_lcdinitialize(&g_ssd1289); + if (!g_ssd1289drvr) + { + lcddbg("ERROR: ssd1289_lcdinitialize failed\n"); + return -ENODEV; + } + } + + /* Clear the display (setting it to the color 0=black) */ + +#if 0 /* Already done in the driver */ + ssd1289_clear(g_ssd1289drvr, 0); +#endif + + /* Turn the display off */ + + g_ssd1289drvr->setpower(g_ssd1289drvr, 0); + return OK; +} + +/************************************************************************************ + * Name: up_lcdgetdev + * + * Description: + * Return a a reference to the LCD object for the specified LCD. This allows + * suport for multiple LCD devices. + * + ************************************************************************************/ + +FAR struct lcd_dev_s *up_lcdgetdev(int lcddev) +{ + DEBUGASSERT(lcddev == 0); + return g_ssd1289drvr; +} + +/************************************************************************************ + * Name: up_lcduninitialize + * + * Description: + * Unitialize the LCD support + * + ************************************************************************************/ + +void up_lcduninitialize(void) +{ + /* Turn the display off */ + + g_ssd1289drvr->setpower(g_ssd1289drvr, 0); +} + +#endif /* CONFIG_LCD_SSD1289 */ From 77d18d52053096080b824f3789aee13554e08399 Mon Sep 17 00:00:00 2001 From: patacongo Date: Mon, 24 Sep 2012 23:27:25 +0000 Subject: [PATCH 31/56] Completes a bit-banging driver for the SSD1289 LCD on the Shenzhou board git-svn-id: http://svn.code.sf.net/p/nuttx/code/trunk@5185 42af7a65-404d-4744-a932-0658087f49c3 --- .../configs/shenzhou/src/shenzhou-internal.h | 122 ++++++++++- nuttx/configs/shenzhou/src/up_spi.c | 24 ++- nuttx/configs/shenzhou/src/up_ssd1289.c | 190 +++++++++--------- 3 files changed, 235 insertions(+), 101 deletions(-) diff --git a/nuttx/configs/shenzhou/src/shenzhou-internal.h b/nuttx/configs/shenzhou/src/shenzhou-internal.h index 32df162ba1..0ac88c4578 100644 --- a/nuttx/configs/shenzhou/src/shenzhou-internal.h +++ b/nuttx/configs/shenzhou/src/shenzhou-internal.h @@ -154,7 +154,7 @@ * -- ---- -------------- ------------------------------------------------------------------- * PN NAME SIGNAL NOTES * -- ---- -------------- ------------------------------------------------------------------- - * 37 PB2 DATA_LE To TFT LCD (CN13, ping 28) + * 37 PB2 DATA_LE To TFT LCD. (CN13, ping 28) * 96 PB9 F_CS To both the TFT LCD (CN13, pin 30) and to the W25X16 SPI FLASH * 34 PC5 TP_INT JP6. To TFT LCD (CN13) module (CN13, pin 26) * 65 PC8 LCD_CS Active low: Pulled high (CN13, pin 19) @@ -185,11 +185,129 @@ * * NOTE: The backlight signl NC_BL (CN13, pin 24) is pulled high and not under * software control + * + * On LCD module: + * -- -------------- ------------------------------------------------------------------- + * PN SIGNAL NOTES + * -- -------------- ------------------------------------------------------------------- + * 3 DB01 To LCD DB1 + * 4 DB00 To LCD DB0 + * 5 DB03 To LCD DB3 + * 6 DB02 To LCD DB2 + * 7 DB05 To LCD DB5 + * 8 DB04 To LCD DB4 + * 9 DB07 To LCD DB7 + * 10 DB06 To LCD DB6 + * 11 DB09 To LCD DB9 + * 12 DB08 To LCD DB8 + * 13 DB11 To LCD DB11 + * 14 DB10 To LCD DB10 + * 15 DB13 To LCD DB13 + * 16 DB12 To LCD DB12 + * 17 DB15 To LCD DB15 + * 18 DB14 To LCD DB14 + * 19 RS To LCD RS + * 20 /LCD_CS To LCD CS + * 21 /RD To LCD RD + * 22 /WR To LCD WR + * 23 BL_EN (Not referenced) + * 24 /RESET + * 25 /INT To Touch IC /INT + * 26 MISO To Touch IC DOUT; To AT45DB161B SO; To SD card DAT0 + * 27 LE To 74HC573 that controls LCD 8-bit/16-bit mode + * 28 MOSI To Touch IC DIN; To AT45DB161B SI; To SD card CMD + * 29 /DF_CS To AT45DB161B Data Flash /CS + * 30 SCLK To Touch IC DCLK; To AT45DB161B SCK; To SD card CLK + * 31 /SD_CS To SD card /CS + * 31 /TP_CS To Touch IC CS */ -#define GPIO_LCD_CS (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ +/* TFT LCD GPIOs */ + +#define GPIO_LCD_D0 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ + GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN0) +#define GPIO_LCD_D1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ + GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN1) +#define GPIO_LCD_D2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ + GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN2) +#define GPIO_LCD_D3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ + GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3) +#define GPIO_LCD_D4 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ + GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN4) +#define GPIO_LCD_D5 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ + GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN5) +#define GPIO_LCD_D6 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ + GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN6) +#define GPIO_LCD_D7 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ + GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN7) +#define GPIO_LCD_D8 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ + GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN8) +#define GPIO_LCD_D9 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ + GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN9) +#define GPIO_LCD_D10 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ + GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN10) +#define GPIO_LCD_D11 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ + GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN11) +#define GPIO_LCD_D12 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ + GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN12) +#define GPIO_LCD_D13 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ + GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN13) +#define GPIO_LCD_D14 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ + GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN14) +#define GPIO_LCD_D15 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ + GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN15) + +#define GPIO_LCD_RS (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ + GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN13) +#define GPIO_LCD_CS (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ + GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN8) +#define GPIO_LCD_RD (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ + GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN15) +#define GPIO_LCD_WR (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ + GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN14) +#define GPIO_LCD_LE (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ + GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN2) + +/* Bit band addresses */ + +#define STM32_GPIOB_OFFSET (STM32_GPIOB_BASE - STM32_PERIPH_BASE) +#define STM32_GPIOC_OFFSET (STM32_GPIOC_BASE - STM32_PERIPH_BASE) +#define STM32_GPIOD_OFFSET (STM32_GPIOD_BASE - STM32_PERIPH_BASE) + +#define LCD_BIT_CLEAR(offs,pin) \ + (STM32_PERIPHBB_BASE + ((offs + STM32_GPIO_BRR_OFFSET) << 5) + ((pin) << 2)) +#define LCD_BIT_SET(offs,pin) \ + (STM32_PERIPHBB_BASE + ((offs + STM32_GPIO_BSRR_OFFSET) << 5) + ((pin) << 2)) + +#define LCD_RS_CLEAR LCD_BIT_CLEAR(STM32_GPIOD_OFFSET, 13) /* GPIO_PORTD|GPIO_PIN13 */ +#define LCD_RS_SET LCD_BIT_SET(STM32_GPIOD_OFFSET, 13) +#define LCD_CS_CLEAR LCD_BIT_CLEAR(STM32_GPIOC_OFFSET, 8) /* GPIO_PORTC|GPIO_PIN8 */ +#define LCD_CS_SET LCD_BIT_SET(STM32_GPIOC_OFFSET, 8) +#define LCD_RD_CLEAR LCD_BIT_CLEAR(STM32_GPIOD_OFFSET, 15) /* GPIO_PORTD|GPIO_PIN15 */ +#define LCD_RD_SET LCD_BIT_SET(STM32_GPIOD_OFFSET, 15) +#define LCD_WR_CLEAR LCD_BIT_CLEAR(STM32_GPIOD_OFFSET, 14) /* GPIO_PORTD|GPIO_PIN14 */ +#define LCD_WR_SET LCD_BIT_SET(STM32_GPIOD_OFFSET, 14) +#define LCD_LE_CLEAR LCD_BIT_CLEAR(STM32_GPIOB_OFFSET, 2) /* GPIO_PORTB|GPIO_PIN2 */ +#define LCD_LE_SET LCD_BIT_SET(STM32_GPIOB_OFFSET, 2) + +#define LCD_DATA STM32_GPIOE_ODR + +/* Touchscreen IC on the LCD module */ + +#define GPIO_TP_INT (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_EXTI|GPIO_PORTC|GPIO_PIN5) +#define GPIO_TP_CS (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ + GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN9) + +/* AT45DB161B Data Flash on the LCD module */ + +#define GPIO_LCDDF_CS (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN9) +/* SD card on the LCD module */ + +#define GPIO_LCDSD_CS (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ + GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN11) + /* RS-485 * * -- ---- -------------- ------------------------------------------------------------------- diff --git a/nuttx/configs/shenzhou/src/up_spi.c b/nuttx/configs/shenzhou/src/up_spi.c index 8f74c92f78..28c5470866 100644 --- a/nuttx/configs/shenzhou/src/up_spi.c +++ b/nuttx/configs/shenzhou/src/up_spi.c @@ -108,10 +108,12 @@ void weak_function stm32_spiinitialize(void) stm32_configgpio(GPIO_FLASH_CS); /* FLASH chip select */ #endif - /* SPI3 connects to TFT LCD and the RF24L01 2.4G wireless module */ + /* SPI3 connects to TFT LCD module and the RF24L01 2.4G wireless module */ #ifdef CONFIG_STM32_SPI3 - stm32_configgpio(GPIO_LCD_CS); /* LCD chip select */ + stm32_configgpio(GPIO_TP_CS); /* Touchscreen chip select */ + stm32_configgpio(GPIO_LCDDF_CS); /* Data flash chip select (on the LCD module) */ + stm32_configgpio(GPIO_LCDSD_CS); /* SD chip select (on the LCD module) */ stm32_configgpio(GPIO_WIRELESS_CS); /* Wireless chip select */ #endif } @@ -182,13 +184,27 @@ void stm32_spi3select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool sele { spidbg("devid: %d CS: %s\n", (int)devid, selected ? "assert" : "de-assert"); - /* SPI3 connects to TFT LCD and the RF24L01 2.4G wireless module */ + /* SPI3 connects to TFT LCD (for touchscreen and SD) and the RF24L01 2.4G + * wireless module. + */ if (devid == SPIDEV_TOUCHSCREEN) { /* Set the GPIO low to select and high to de-select */ - stm32_gpiowrite(GPIO_LCD_CS, !selected); + stm32_gpiowrite(GPIO_TP_CS, !selected); + } + else if (devid == SPIDEV_MMCSD) + { + /* Set the GPIO low to select and high to de-select */ + + stm32_gpiowrite(GPIO_LCDDF_CS, !selected); + } + else if (devid == SPIDEV_FLASH) + { + /* Set the GPIO low to select and high to de-select */ + + stm32_gpiowrite(GPIO_LCDSD_CS, !selected); } else if (devid == SPIDEV_WIRELESS) { diff --git a/nuttx/configs/shenzhou/src/up_ssd1289.c b/nuttx/configs/shenzhou/src/up_ssd1289.c index 6c24e54042..42a0ffbd20 100644 --- a/nuttx/configs/shenzhou/src/up_ssd1289.c +++ b/nuttx/configs/shenzhou/src/up_ssd1289.c @@ -69,8 +69,8 @@ ************************************************************************************/ /* Configuration ********************************************************************/ -#ifndef CONFIG_STM32_FSMC -# error "CONFIG_STM32_FSMC is required to use the LCD" +#ifndef CONFIG_SSD1289_WRONLY +# warning "Only write access is supported; CONFIG_SSD1289_WRONLY should be defined" #endif /* Define CONFIG_DEBUG_LCD to enable detailed LCD debug output. Verbose debug must @@ -88,26 +88,6 @@ #endif /* Shenzhou LCD Hardware Definitions ************************************************/ -/* LCD /CS is CE1 == NOR/SRAM Bank 1 - * - * Bank 1 = 0x60000000 | 0x00000000 - * Bank 2 = 0x60000000 | 0x04000000 - * Bank 3 = 0x60000000 | 0x08000000 - * Bank 4 = 0x60000000 | 0x0c000000 - * - * FSMC address bit 16 is used to distinguish command and data. FSMC address bits - * 0-24 correspond to ARM address bits 1-25. - */ - -#define STM32_LCDBASE ((uintptr_t)(0x60000000 | 0x00000000)) -#define LCD_INDEX (STM32_LCDBASE) -#define LCD_DATA (STM32_LCDBASE + 0x00020000) - -/* SRAM pin definitions */ - -#define LCD_NADDRLINES 1 /* A16 */ -#define LCD_NDATALINES 16 /* D0-15 */ - /* Debug ****************************************************************************/ #ifdef CONFIG_DEBUG_LCD @@ -125,6 +105,10 @@ /************************************************************************************** * Private Function Protototypes **************************************************************************************/ +/* Helpers */ + +static void stm32_wrdata(uint16_t data); + /* Low Level LCD access */ static void stm32_select(FAR struct ssd1289_lcd_s *dev); @@ -175,18 +159,54 @@ static void stm32_backlight(FAR struct ssd1289_lcd_s *dev, int power); * * NOTE: The backlight signl NC_BL (CN13, pin 24) is pulled high and not under * software control + * + * On LCD module: + * -- -------------- ------------------------------------------------------------------- + * PN SIGNAL NOTES + * -- -------------- ------------------------------------------------------------------- + * 3 DB01 To LCD DB1 + * 4 DB00 To LCD DB0 + * 5 DB03 To LCD DB3 + * 6 DB02 To LCD DB2 + * 7 DB05 To LCD DB5 + * 8 DB04 To LCD DB4 + * 9 DB07 To LCD DB7 + * 10 DB06 To LCD DB6 + * 11 DB09 To LCD DB9 + * 12 DB08 To LCD DB8 + * 13 DB11 To LCD DB11 + * 14 DB10 To LCD DB10 + * 15 DB13 To LCD DB13 + * 16 DB12 To LCD DB12 + * 17 DB15 To LCD DB15 + * 18 DB14 To LCD DB14 + * 19 RS To LCD RS + * 20 /LCD_CS To LCD CS + * 21 /RD To LCD RD + * 22 /WR To LCD WR + * 23 BL_EN (Not referenced) + * 24 /RESET + * 25 /INT To Touch IC /INT + * 26 MISO To Touch IC DOUT; To AT45DB161B SO; To SD card DAT0 + * 27 LE To 74HC573 that controls LCD 8-bit/16-bit mode + * 28 MOSI To Touch IC DIN; To AT45DB161B SI; To SD card CMD + * 29 /DF_CS To AT45DB161B Data Flash /CS + * 30 SCLK To Touch IC DCLK; To AT45DB161B SCK; To SD card CLK + * 31 /SD_CS To SD card /CS + * 31 /TP_CS To Touch IC CS */ -#define GPIO_LCD_RESET (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\ - GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN6) - -/* GPIO configurations unique to the LCD */ +/* LCD GPIO configurations */ static const uint32_t g_lcdconfig[] = { - /* PC6(RESET), FSMC_A16, FSMC_NOE, FSMC_NWE, and FSMC_NE1 */ + GPIO_LCD_D0, GPIO_LCD_D1, GPIO_LCD_D2, GPIO_LCD_D3, + GPIO_LCD_D4, GPIO_LCD_D5, GPIO_LCD_D6, GPIO_LCD_D7, + GPIO_LCD_D8, GPIO_LCD_D9, GPIO_LCD_D10, GPIO_LCD_D11, + GPIO_LCD_D12, GPIO_LCD_D13, GPIO_LCD_D14, GPIO_LCD_D15, - GPIO_LCD_RESET, GPIO_FSMC_A16, GPIO_FSMC_NOE, GPIO_FSMC_NWE, GPIO_FSMC_NE1 + GPIO_LCD_RS, GPIO_LCD_CS, GPIO_LCD_RD, GPIO_LCD_WR, + GPIO_LCD_LE, }; #define NLCD_CONFIG (sizeof(g_lcdconfig)/sizeof(uint32_t)) @@ -212,6 +232,23 @@ static FAR struct lcd_dev_s *g_ssd1289drvr; * Private Functions ************************************************************************************/ +/************************************************************************************ + * Name: stm32_wrdata + * + * Description: + * Latch data on D0-D15 and toggle the WR line. + * + ************************************************************************************/ + +static void stm32_wrdata(uint16_t data) +{ + /* Latch the 16-bit LCD data and toggle the WR line */ + + putreg32((uint32_t)data, LCD_DATA); + putreg32(1, LCD_WR_CLEAR); + putreg32(1, LCD_WR_SET); +} + /************************************************************************************ * Name: stm32_select * @@ -222,7 +259,9 @@ static FAR struct lcd_dev_s *g_ssd1289drvr; static void stm32_select(FAR struct ssd1289_lcd_s *dev) { - /* Does not apply to this hardware */ + /* Select the LCD by setting the LCD_CS low */ + + putreg32(1, LCD_CS_CLEAR); } /************************************************************************************ @@ -235,7 +274,9 @@ static void stm32_select(FAR struct ssd1289_lcd_s *dev) static void stm32_deselect(FAR struct ssd1289_lcd_s *dev) { - /* Does not apply to this hardware */ + /* De-select the LCD by setting the LCD_CS high */ + + putreg32(1, LCD_CS_SET); } /************************************************************************************ @@ -248,7 +289,13 @@ static void stm32_deselect(FAR struct ssd1289_lcd_s *dev) static void stm32_index(FAR struct ssd1289_lcd_s *dev, uint8_t index) { - putreg16((uint16_t)index, LCD_INDEX); + /* Clear the RS signal */ + + putreg32(1, LCD_RS_CLR); + + /* And write the index */ + + stm32_wrdata((uint16_t)index); } /************************************************************************************ @@ -262,7 +309,7 @@ static void stm32_index(FAR struct ssd1289_lcd_s *dev, uint8_t index) #ifndef CONFIG_SSD1289_WRONLY static uint16_t stm32_read(FAR struct ssd1289_lcd_s *dev) { - return getreg16(LCD_DATA); +#warning "Missing logic" } #endif @@ -276,11 +323,17 @@ static uint16_t stm32_read(FAR struct ssd1289_lcd_s *dev) static void stm32_write(FAR struct ssd1289_lcd_s *dev, uint16_t data) { - putreg16((uint16_t)data, LCD_DATA); + /* Set the RS signal */ + + putreg32(1, LCD_RS_CLR); + + /* And write the data */ + + stm32_wrdata(data); } /************************************************************************************ - * Name: stm32_write + * Name: stm32_backlight * * Description: * Write LCD data (GRAM data or register contents) @@ -289,52 +342,7 @@ static void stm32_write(FAR struct ssd1289_lcd_s *dev, uint16_t data) static void stm32_backlight(FAR struct ssd1289_lcd_s *dev, int power) { -#warning "Missing logic" -} - -/************************************************************************************ - * Name: stm32_selectlcd - * - * Description: - * Initialize to the LCD - * - ************************************************************************************/ - -void stm32_selectlcd(void) -{ - /* Configure GPIO pins */ - - stm32_extmemdata(LCD_NDATALINES); /* Common data lines: D0-D15 */ - stm32_extmemgpios(g_lcdconfig, NLCD_CONFIG); /* LCD-specific control lines */ - - /* Enable AHB clocking to the FSMC */ - - stm32_enablefsmc(); - - /* Color LCD configuration (LCD configured as follow): - * - * - Data/Address MUX = Disable "FSMC_BCR_MUXEN" just not enable it. - * - Extended Mode = Disable "FSMC_BCR_EXTMOD" - * - Memory Type = SRAM "FSMC_BCR_SRAM" - * - Data Width = 16bit "FSMC_BCR_MWID16" - * - Write Operation = Enable "FSMC_BCR_WREN" - * - Asynchronous Wait = Disable - */ - - /* Bank1 NOR/SRAM control register configuration */ - - putreg32(FSMC_BCR_SRAM | FSMC_BCR_MWID16 | FSMC_BCR_WREN, STM32_FSMC_BCR1); - - /* Bank1 NOR/SRAM timing register configuration */ - - putreg32(FSMC_BTR_ADDSET(5) | FSMC_BTR_ADDHLD(0) | FSMC_BTR_DATAST(9) | FSMC_BTR_BUSTRUN(0) | - FSMC_BTR_CLKDIV(0) | FSMC_BTR_DATLAT(0) | FSMC_BTR_ACCMODA, STM32_FSMC_BTR1); - - putreg32(0xffffffff, STM32_FSMC_BWTR1); - - /* Enable the bank by setting the MBKEN bit */ - - putreg32(FSMC_BCR_MBKEN | FSMC_BCR_SRAM | FSMC_BCR_MWID16 | FSMC_BCR_WREN, STM32_FSMC_BCR1); + /* There is no software control over the backlight */ } /************************************************************************************ @@ -353,25 +361,23 @@ void stm32_selectlcd(void) int up_lcdinitialize(void) { + int i; + /* Only initialize the driver once */ if (!g_ssd1289drvr) { lcdvdbg("Initializing\n"); - /* Configure GPIO pins and configure the FSMC to support the LCD */ + /* Configure GPIO pins */ - stm32_selectlcd(); - - /* Reset the LCD (active low) */ - - stm32_gpiowrite(GPIO_LCD_RESET, false); - up_mdelay(5); - stm32_gpiowrite(GPIO_LCD_RESET, true); + for (i = 0; i < NLCD_CONFIG; i++) + { + stm32_configgpio(g_lcdconfig[i]); + } /* Configure and enable the LCD */ - up_mdelay(50); g_ssd1289drvr = ssd1289_lcdinitialize(&g_ssd1289); if (!g_ssd1289drvr) { @@ -380,12 +386,6 @@ int up_lcdinitialize(void) } } - /* Clear the display (setting it to the color 0=black) */ - -#if 0 /* Already done in the driver */ - ssd1289_clear(g_ssd1289drvr, 0); -#endif - /* Turn the display off */ g_ssd1289drvr->setpower(g_ssd1289drvr, 0); From 4c432c716a57b1f6a7c7609a7bc36b6e0a49e2e0 Mon Sep 17 00:00:00 2001 From: patacongo Date: Tue, 25 Sep 2012 01:14:43 +0000 Subject: [PATCH 32/56] Beginning of an Shenshou NxWM configuration git-svn-id: http://svn.code.sf.net/p/nuttx/code/trunk@5186 42af7a65-404d-4744-a932-0658087f49c3 --- nuttx/ChangeLog | 2 + nuttx/configs/shenzhou/nxwm/Make.defs | 168 +++++++ nuttx/configs/shenzhou/nxwm/setenv.sh | 78 ++++ nuttx/configs/stm3220g-eval/dhcpd/defconfig | 2 +- nuttx/configs/stm3220g-eval/nettest/defconfig | 2 +- nuttx/configs/stm3220g-eval/nsh/defconfig | 2 +- nuttx/configs/stm3220g-eval/nsh2/defconfig | 2 +- nuttx/configs/stm3220g-eval/nxwm/defconfig | 2 +- nuttx/configs/stm3220g-eval/ostest/defconfig | 2 +- nuttx/configs/stm3220g-eval/telnetd/defconfig | 2 +- nuttx/configs/stm3240g-eval/dhcpd/defconfig | 2 +- nuttx/configs/stm3240g-eval/nettest/defconfig | 2 +- nuttx/configs/stm3240g-eval/nsh/defconfig | 2 +- nuttx/configs/stm3240g-eval/nsh2/defconfig | 2 +- .../configs/stm3240g-eval/nxconsole/defconfig | 2 +- nuttx/configs/stm3240g-eval/nxwm/defconfig | 2 +- nuttx/configs/stm3240g-eval/ostest/defconfig | 2 +- nuttx/configs/stm3240g-eval/telnetd/defconfig | 2 +- .../configs/stm3240g-eval/webserver/defconfig | 2 +- nuttx/configs/stm32f4discovery/nsh/defconfig | 2 +- .../stm32f4discovery/nxlines/defconfig | 2 +- .../configs/stm32f4discovery/ostest/defconfig | 2 +- nuttx/graphics/Kconfig | 426 ++++++++++++++++++ 23 files changed, 693 insertions(+), 19 deletions(-) create mode 100644 nuttx/configs/shenzhou/nxwm/Make.defs create mode 100755 nuttx/configs/shenzhou/nxwm/setenv.sh diff --git a/nuttx/ChangeLog b/nuttx/ChangeLog index b7f1bc949d..895f2844fc 100644 --- a/nuttx/ChangeLog +++ b/nuttx/ChangeLog @@ -3395,3 +3395,5 @@ STM32F4Discovery's FSMC-based LCD interface. The Shenzhou will need a completely need bit-banging interface; this initial check-in is only for the framework. + * configs/shenzhou/src/up_ssd1289.c: Bit-banging driver is + code complete. \ No newline at end of file diff --git a/nuttx/configs/shenzhou/nxwm/Make.defs b/nuttx/configs/shenzhou/nxwm/Make.defs new file mode 100644 index 0000000000..0025282b44 --- /dev/null +++ b/nuttx/configs/shenzhou/nxwm/Make.defs @@ -0,0 +1,168 @@ +############################################################################ +# configs/shenzhou/nxwm/Make.defs +# +# Copyright (C) 2012 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. +# +############################################################################ + +include ${TOPDIR}/.config +include ${TOPDIR}/tools/Config.mk + +# Setup for the selected toolchain + +ifeq ($(CONFIG_STM32_CODESOURCERYW),y) + # CodeSourcery under Windows + CROSSDEV = arm-none-eabi- + ARCROSSDEV = arm-none-eabi- + WINTOOL = y + ARCHCPUFLAGS = -mcpu=cortex-m3 -mthumb -mfloat-abi=soft +endif +ifeq ($(CONFIG_STM32_CODESOURCERYL),y) + # CodeSourcery under Linux + CROSSDEV = arm-none-eabi- + ARCROSSDEV = arm-none-eabi- + ARCHCPUFLAGS = -mcpu=cortex-m3 -mthumb -mfloat-abi=soft + MAXOPTIMIZATION = -O2 +endif +ifeq ($(CONFIG_STM32_ATOLLIC_LITE),y) + # Atollic toolchain under Windows + CROSSDEV = arm-atollic-eabi- + ARCROSSDEV = + WINTOOL = y + ARCHCPUFLAGS = -mcpu=cortex-m3 -mthumb -mfloat-abi=soft +endif +ifeq ($(CONFIG_STM32_ATOLLIC_PRO),y) + # Atollic toolchain under Windows + CROSSDEV = arm-atollic-eabi- + ARCROSSDEV = arm-atollic-eabi- + WINTOOL = y + ARCHCPUFLAGS = -mcpu=cortex-m3 -mthumb -mfloat-abi=soft +endif +ifeq ($(CONFIG_STM32_DEVKITARM),y) + # devkitARM under Windows + CROSSDEV = arm-eabi- + ARCROSSDEV = arm-eabi- + WINTOOL = y + ARCHCPUFLAGS = -mcpu=cortex-m3 -mthumb -mfloat-abi=soft +endif +ifeq ($(CONFIG_STM32_RAISONANCE),y) + # Raisonance RIDE7 under Windows + CROSSDEV = arm-none-eabi- + ARCROSSDEV = arm-none-eabi- + WINTOOL = y + ARCHCPUFLAGS = -mcpu=cortex-m3 -mthumb -mfloat-abi=soft +endif +ifeq ($(CONFIG_STM32_BUILDROOT),y) + # NuttX buildroot under Linux or Cygwin + CROSSDEV = arm-elf- + ARCROSSDEV = arm-elf- + ARCHCPUFLAGS = -mtune=cortex-m3 -march=armv7-m -mfloat-abi=soft + MAXOPTIMIZATION = -Os +endif + +# Pick the linker script + +ifeq ($(CONFIG_STM32_DFU),y) + LDSCRIPT = ld.script.dfu +else + LDSCRIPT = ld.script +endif + +ifeq ($(WINTOOL),y) + # Windows-native toolchains + DIRLINK = $(TOPDIR)/tools/winlink.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" + ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}" + ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)}" + MAXOPTIMIZATION = -O2 +else + # Linux/Cygwin-native toolchain + MKDEP = $(TOPDIR)/tools/mkdeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) +endif + +CC = $(CROSSDEV)gcc +CXX = $(CROSSDEV)g++ +CPP = $(CROSSDEV)gcc -E +LD = $(CROSSDEV)ld +AR = $(ARCROSSDEV)ar rcs +NM = $(ARCROSSDEV)nm +OBJCOPY = $(CROSSDEV)objcopy +OBJDUMP = $(CROSSDEV)objdump + +ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'} +ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1} + +ifeq ("${CONFIG_DEBUG_SYMBOLS}","y") + ARCHOPTIMIZATION = -g +else + ARCHOPTIMIZATION = $(MAXOPTIMIZATION) -fno-strict-aliasing -fno-strength-reduce -fomit-frame-pointer +endif + +ARCHCFLAGS = -fno-builtin +ARCHCXXFLAGS = -fno-builtin -fno-exceptions -fno-rtti +ARCHWARNINGS = -Wall -Wstrict-prototypes -Wshadow +ARCHWARNINGSXX = -Wall -Wshadow +ARCHDEFINES = +ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 + +CFLAGS = $(ARCHCFLAGS) $(ARCHWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe +CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS) +CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe +CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS) +CPPFLAGS = $(ARCHINCLUDES) $(ARCHDEFINES) $(EXTRADEFINES) +AFLAGS = $(CFLAGS) -D__ASSEMBLY__ + +NXFLATLDFLAGS1 = -r -d -warn-common +NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections +LDNXFLATFLAGS = -e main -s 2048 + +OBJEXT = .o +LIBEXT = .a +EXEEXT = + +ifneq ($(CROSSDEV),arm-elf-) + LDFLAGS += -nostartfiles -nodefaultlibs +endif +ifeq ($(CONFIG_DEBUG_SYMBOLS),y) + LDFLAGS += -g +endif + + +HOSTCC = gcc +HOSTINCLUDES = -I. +HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe +HOSTLDFLAGS = + diff --git a/nuttx/configs/shenzhou/nxwm/setenv.sh b/nuttx/configs/shenzhou/nxwm/setenv.sh new file mode 100755 index 0000000000..7f00448a60 --- /dev/null +++ b/nuttx/configs/shenzhou/nxwm/setenv.sh @@ -0,0 +1,78 @@ +#!/bin/bash +# configs/shenzhou/nxwm/setenv.sh +# +# Copyright (C) 2012 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. +# + +if [ "$_" = "$0" ] ; then + echo "You must source this script, not run it!" 1>&2 + exit 1 +fi + +WD=`pwd` +if [ ! -x "setenv.sh" ]; then + echo "This script must be executed from the top-level NuttX build directory" + exit 1 +fi + +if [ -z "${PATH_ORIG}" ]; then + export PATH_ORIG="${PATH}" +fi + +# This the Cygwin path to the location where I installed the RIDE +# toolchain under windows. You will also have to edit this if you install +# the RIDE toolchain in any other location +#export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/Raisonance/Ride/arm-gcc/bin" + +# This the Cygwin path to the location where I installed the CodeSourcery +# toolchain under windows. You will also have to edit this if you install +# the CodeSourcery toolchain in any other location +export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/CodeSourcery/Sourcery G++ Lite/bin" + +# These are the Cygwin paths to the locations where I installed the Atollic +# toolchain under windows. You will also have to edit this if you install +# the Atollic toolchain in any other location. /usr/bin is added before +# the Atollic bin path because there is are binaries named gcc.exe and g++.exe +# at those locations as well. +#export TOOLCHAIN_BIN="/usr/bin:/cygdrive/c/Program Files (x86)/Atollic/TrueSTUDIO for ARM Pro 2.3.0/ARMTools/bin" +#export TOOLCHAIN_BIN="/usr/bin:/cygdrive/c/Program Files (x86)/Atollic/TrueSTUDIO for STMicroelectronics STM32 Lite 2.3.0/ARMTools/bin" + +# This the Cygwin path to the location where I build the buildroot +# toolchain. +#export TOOLCHAIN_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin" + +# This is the path to the tools/ subdirectory +export TOOLS_DIR="${WD}/configs/shenzhou/tools" + +# Add the path to the toolchain to the PATH variable +export PATH="${TOOLCHAIN_BIN}:${TOOLS_DIR}:/sbin:/usr/sbin:${PATH_ORIG}" + +echo "PATH : ${PATH}" diff --git a/nuttx/configs/stm3220g-eval/dhcpd/defconfig b/nuttx/configs/stm3220g-eval/dhcpd/defconfig index 3958aa8015..d011a3bfb0 100644 --- a/nuttx/configs/stm3220g-eval/dhcpd/defconfig +++ b/nuttx/configs/stm3220g-eval/dhcpd/defconfig @@ -422,7 +422,7 @@ CONFIG_NET_RESOLV_ENTRIES=4 CONFIG_RTC=n CONFIG_RTC_DATETIME=y CONFIG_RTC_HIRES=n -CONFIG_RTC_FREQUENCY=n +CONFIG_RTC_FREQUENCY=1 CONFIG_RTC_ALARM=n # diff --git a/nuttx/configs/stm3220g-eval/nettest/defconfig b/nuttx/configs/stm3220g-eval/nettest/defconfig index 0b19651cd4..48a7982c2a 100644 --- a/nuttx/configs/stm3220g-eval/nettest/defconfig +++ b/nuttx/configs/stm3220g-eval/nettest/defconfig @@ -422,7 +422,7 @@ CONFIG_NET_RESOLV_ENTRIES=4 CONFIG_RTC=n CONFIG_RTC_DATETIME=y CONFIG_RTC_HIRES=n -CONFIG_RTC_FREQUENCY=n +CONFIG_RTC_FREQUENCY=1 CONFIG_RTC_ALARM=n # diff --git a/nuttx/configs/stm3220g-eval/nsh/defconfig b/nuttx/configs/stm3220g-eval/nsh/defconfig index c2cf20a5ca..1f8cac1f14 100644 --- a/nuttx/configs/stm3220g-eval/nsh/defconfig +++ b/nuttx/configs/stm3220g-eval/nsh/defconfig @@ -473,7 +473,7 @@ CONFIG_FTPD_CMDBUFFERSIZE=2048 CONFIG_RTC=y CONFIG_RTC_DATETIME=y CONFIG_RTC_HIRES=n -CONFIG_RTC_FREQUENCY=n +CONFIG_RTC_FREQUENCY=1 CONFIG_RTC_ALARM=n # diff --git a/nuttx/configs/stm3220g-eval/nsh2/defconfig b/nuttx/configs/stm3220g-eval/nsh2/defconfig index de7e59bca2..8462ef3766 100644 --- a/nuttx/configs/stm3220g-eval/nsh2/defconfig +++ b/nuttx/configs/stm3220g-eval/nsh2/defconfig @@ -472,7 +472,7 @@ CONFIG_FTPD_CMDBUFFERSIZE=2048 CONFIG_RTC=y CONFIG_RTC_DATETIME=y CONFIG_RTC_HIRES=n -CONFIG_RTC_FREQUENCY=n +CONFIG_RTC_FREQUENCY=1 CONFIG_RTC_ALARM=n # diff --git a/nuttx/configs/stm3220g-eval/nxwm/defconfig b/nuttx/configs/stm3220g-eval/nxwm/defconfig index 0182a15d6b..7b7028cf84 100644 --- a/nuttx/configs/stm3220g-eval/nxwm/defconfig +++ b/nuttx/configs/stm3220g-eval/nxwm/defconfig @@ -474,7 +474,7 @@ CONFIG_FTPD_CMDBUFFERSIZE=2048 CONFIG_RTC=y CONFIG_RTC_DATETIME=y CONFIG_RTC_HIRES=n -CONFIG_RTC_FREQUENCY=n +CONFIG_RTC_FREQUENCY=1 CONFIG_RTC_ALARM=n # diff --git a/nuttx/configs/stm3220g-eval/ostest/defconfig b/nuttx/configs/stm3220g-eval/ostest/defconfig index 46e82d6e1a..5fc04f03c3 100644 --- a/nuttx/configs/stm3220g-eval/ostest/defconfig +++ b/nuttx/configs/stm3220g-eval/ostest/defconfig @@ -409,7 +409,7 @@ CONFIG_NET_RESOLV_ENTRIES=4 CONFIG_RTC=n CONFIG_RTC_DATETIME=y CONFIG_RTC_HIRES=n -CONFIG_RTC_FREQUENCY=n +CONFIG_RTC_FREQUENCY=1 CONFIG_RTC_ALARM=n # diff --git a/nuttx/configs/stm3220g-eval/telnetd/defconfig b/nuttx/configs/stm3220g-eval/telnetd/defconfig index 823f455c1c..e369a522ef 100644 --- a/nuttx/configs/stm3220g-eval/telnetd/defconfig +++ b/nuttx/configs/stm3220g-eval/telnetd/defconfig @@ -422,7 +422,7 @@ CONFIG_NET_RESOLV_ENTRIES=4 CONFIG_RTC=n CONFIG_RTC_DATETIME=y CONFIG_RTC_HIRES=n -CONFIG_RTC_FREQUENCY=n +CONFIG_RTC_FREQUENCY=1 CONFIG_RTC_ALARM=n # diff --git a/nuttx/configs/stm3240g-eval/dhcpd/defconfig b/nuttx/configs/stm3240g-eval/dhcpd/defconfig index 09bab66b9c..842e0c66be 100644 --- a/nuttx/configs/stm3240g-eval/dhcpd/defconfig +++ b/nuttx/configs/stm3240g-eval/dhcpd/defconfig @@ -428,7 +428,7 @@ CONFIG_NET_RESOLV_ENTRIES=4 CONFIG_RTC=n CONFIG_RTC_DATETIME=y CONFIG_RTC_HIRES=n -CONFIG_RTC_FREQUENCY=n +CONFIG_RTC_FREQUENCY=1 CONFIG_RTC_ALARM=n # diff --git a/nuttx/configs/stm3240g-eval/nettest/defconfig b/nuttx/configs/stm3240g-eval/nettest/defconfig index 2560310da4..55ac141577 100644 --- a/nuttx/configs/stm3240g-eval/nettest/defconfig +++ b/nuttx/configs/stm3240g-eval/nettest/defconfig @@ -428,7 +428,7 @@ CONFIG_NET_RESOLV_ENTRIES=4 CONFIG_RTC=n CONFIG_RTC_DATETIME=y CONFIG_RTC_HIRES=n -CONFIG_RTC_FREQUENCY=n +CONFIG_RTC_FREQUENCY=1 CONFIG_RTC_ALARM=n # diff --git a/nuttx/configs/stm3240g-eval/nsh/defconfig b/nuttx/configs/stm3240g-eval/nsh/defconfig index dad3b38543..80f5ec167c 100644 --- a/nuttx/configs/stm3240g-eval/nsh/defconfig +++ b/nuttx/configs/stm3240g-eval/nsh/defconfig @@ -478,7 +478,7 @@ CONFIG_FTPD_CMDBUFFERSIZE=2048 CONFIG_RTC=y CONFIG_RTC_DATETIME=y CONFIG_RTC_HIRES=n -CONFIG_RTC_FREQUENCY=n +CONFIG_RTC_FREQUENCY=1 CONFIG_RTC_ALARM=n # diff --git a/nuttx/configs/stm3240g-eval/nsh2/defconfig b/nuttx/configs/stm3240g-eval/nsh2/defconfig index 86504cc835..9d25910681 100644 --- a/nuttx/configs/stm3240g-eval/nsh2/defconfig +++ b/nuttx/configs/stm3240g-eval/nsh2/defconfig @@ -478,7 +478,7 @@ CONFIG_FTPD_CMDBUFFERSIZE=2048 CONFIG_RTC=y CONFIG_RTC_DATETIME=y CONFIG_RTC_HIRES=n -CONFIG_RTC_FREQUENCY=n +CONFIG_RTC_FREQUENCY=1 CONFIG_RTC_ALARM=n # diff --git a/nuttx/configs/stm3240g-eval/nxconsole/defconfig b/nuttx/configs/stm3240g-eval/nxconsole/defconfig index 70f7465b00..e7d27bd2ec 100644 --- a/nuttx/configs/stm3240g-eval/nxconsole/defconfig +++ b/nuttx/configs/stm3240g-eval/nxconsole/defconfig @@ -477,7 +477,7 @@ CONFIG_FTPD_CMDBUFFERSIZE=2048 CONFIG_RTC=y CONFIG_RTC_DATETIME=y CONFIG_RTC_HIRES=n -CONFIG_RTC_FREQUENCY=n +CONFIG_RTC_FREQUENCY=1 CONFIG_RTC_ALARM=n # diff --git a/nuttx/configs/stm3240g-eval/nxwm/defconfig b/nuttx/configs/stm3240g-eval/nxwm/defconfig index ec98c273b5..c90051b9b4 100644 --- a/nuttx/configs/stm3240g-eval/nxwm/defconfig +++ b/nuttx/configs/stm3240g-eval/nxwm/defconfig @@ -479,7 +479,7 @@ CONFIG_FTPD_CMDBUFFERSIZE=2048 CONFIG_RTC=y CONFIG_RTC_DATETIME=y CONFIG_RTC_HIRES=n -CONFIG_RTC_FREQUENCY=n +CONFIG_RTC_FREQUENCY=1 CONFIG_RTC_ALARM=n # diff --git a/nuttx/configs/stm3240g-eval/ostest/defconfig b/nuttx/configs/stm3240g-eval/ostest/defconfig index 33bb63d5da..bfc576f5fc 100644 --- a/nuttx/configs/stm3240g-eval/ostest/defconfig +++ b/nuttx/configs/stm3240g-eval/ostest/defconfig @@ -414,7 +414,7 @@ CONFIG_NET_RESOLV_ENTRIES=4 CONFIG_RTC=n CONFIG_RTC_DATETIME=y CONFIG_RTC_HIRES=n -CONFIG_RTC_FREQUENCY=n +CONFIG_RTC_FREQUENCY=1 CONFIG_RTC_ALARM=n # diff --git a/nuttx/configs/stm3240g-eval/telnetd/defconfig b/nuttx/configs/stm3240g-eval/telnetd/defconfig index 5609f0d18d..f138e5e6f1 100644 --- a/nuttx/configs/stm3240g-eval/telnetd/defconfig +++ b/nuttx/configs/stm3240g-eval/telnetd/defconfig @@ -428,7 +428,7 @@ CONFIG_NET_RESOLV_ENTRIES=4 CONFIG_RTC=n CONFIG_RTC_DATETIME=y CONFIG_RTC_HIRES=n -CONFIG_RTC_FREQUENCY=n +CONFIG_RTC_FREQUENCY=1 CONFIG_RTC_ALARM=n # diff --git a/nuttx/configs/stm3240g-eval/webserver/defconfig b/nuttx/configs/stm3240g-eval/webserver/defconfig index 0bde916e47..25f1800b6d 100644 --- a/nuttx/configs/stm3240g-eval/webserver/defconfig +++ b/nuttx/configs/stm3240g-eval/webserver/defconfig @@ -478,7 +478,7 @@ CONFIG_FTPD_CMDBUFFERSIZE=2048 CONFIG_RTC=y CONFIG_RTC_DATETIME=y CONFIG_RTC_HIRES=n -CONFIG_RTC_FREQUENCY=n +CONFIG_RTC_FREQUENCY=1 CONFIG_RTC_ALARM=n # diff --git a/nuttx/configs/stm32f4discovery/nsh/defconfig b/nuttx/configs/stm32f4discovery/nsh/defconfig index a5c5035910..38a0a1e038 100644 --- a/nuttx/configs/stm32f4discovery/nsh/defconfig +++ b/nuttx/configs/stm32f4discovery/nsh/defconfig @@ -442,7 +442,7 @@ CONFIG_NET_RESOLV_ENTRIES=4 CONFIG_RTC=n CONFIG_RTC_DATETIME=y CONFIG_RTC_HIRES=n -CONFIG_RTC_FREQUENCY=n +CONFIG_RTC_FREQUENCY=1 CONFIG_RTC_ALARM=n # diff --git a/nuttx/configs/stm32f4discovery/nxlines/defconfig b/nuttx/configs/stm32f4discovery/nxlines/defconfig index c6bd3f5df6..bbdbf0feae 100644 --- a/nuttx/configs/stm32f4discovery/nxlines/defconfig +++ b/nuttx/configs/stm32f4discovery/nxlines/defconfig @@ -442,7 +442,7 @@ CONFIG_NET_RESOLV_ENTRIES=4 CONFIG_RTC=n CONFIG_RTC_DATETIME=y CONFIG_RTC_HIRES=n -CONFIG_RTC_FREQUENCY=n +CONFIG_RTC_FREQUENCY=1 CONFIG_RTC_ALARM=n # diff --git a/nuttx/configs/stm32f4discovery/ostest/defconfig b/nuttx/configs/stm32f4discovery/ostest/defconfig index 39675a166a..a10efe9eca 100644 --- a/nuttx/configs/stm32f4discovery/ostest/defconfig +++ b/nuttx/configs/stm32f4discovery/ostest/defconfig @@ -392,7 +392,7 @@ CONFIG_NET_RESOLV_ENTRIES=4 CONFIG_RTC=n CONFIG_RTC_DATETIME=y CONFIG_RTC_HIRES=n -CONFIG_RTC_FREQUENCY=n +CONFIG_RTC_FREQUENCY=1 CONFIG_RTC_ALARM=n # diff --git a/nuttx/graphics/Kconfig b/nuttx/graphics/Kconfig index ae2bf31307..8af3c2d1ef 100644 --- a/nuttx/graphics/Kconfig +++ b/nuttx/graphics/Kconfig @@ -2,3 +2,429 @@ # For a description of the syntax of this configuration file, # see misc/tools/kconfig-language.txt. # + +config NX + bool "NX Graphics" + default n + ---help--- + Enables overall support for graphics library and NX + +if NX + +config NX_NPLANES + int "Number of Color Planes" + default 1 + ---help--- + Some YUV color formats requires support for multiple planes, one for each + color component. Unless you have such special hardware, this value should be + undefined or set to 1. + +config NX_WRITEONLY + bool "Write-only Graphics Device" + default y if NX_LCDDRIVER && NX_LCDDRIVER + default n if !NX_LCDDRIVER || !NX_LCDDRIVER + ---help--- + Define if the underlying graphics device does not support read operations. + Automatically defined if NX_LCDDRIVER and LCD_NOGETRUN are + defined. + +menu "Supported Pixel Depths" + +config NX_DISABLE_1BPP + bool "1 BPP" + default y + ---help--- + NX supports a variety of pixel depths. You can save some memory by disabling + support for unused color depths. The selection disables support for 1BPP + pixel depth. + +config NX_DISABLE_2BPP + bool "2 BPP" + default y + ---help--- + NX supports a variety of pixel depths. You can save some memory by disabling + support for unused color depths. The selection disables support for 2BPP + pixel depth. + +config NX_DISABLE_4BPP + bool "4 BPP" + default y + ---help--- + NX supports a variety of pixel depths. You can save some memory by disabling + support for unused color depths. The selection disables support for 4BPP + pixel depth. + +config NX_DISABLE_8BPP + bool "8 BPP" + default y + ---help--- + NX supports a variety of pixel depths. You can save some memory by disabling + support for unused color depths. The selection disables support for 8BPP + pixel depth. + +config NX_DISABLE_16BPP + bool "16 BPP" + default y + ---help--- + NX supports a variety of pixel depths. You can save some memory by disabling + support for unused color depths. The selection disables support for 16BPP + pixel depth. + +config NX_DISABLE_24BPP + bool "24 BPP" + default y + ---help--- + NX supports a variety of pixel depths. You can save some memory by disabling + support for unused color depths. The selection disables support for 24BPP + pixel depth. + +config NX_DISABLE_32BPP + bool "32 BPP" + default y + ---help--- + NX supports a variety of pixel depths. You can save some memory by disabling + support for unused color depths. The selection disables support for 32BPP + pixel depth. + +endmenu + +config NX_PACKEDMSFIRST + bool "Packed MS First" + default y + depends on NX_DISABLE_1BPP || NX_DISABLE_2BPP || NX_DISABLE_4BPP + ---help--- + If a pixel depth of less than 8-bits is used, then NX needs to know if the + pixels pack from the MS to LS or from LS to MS + +menu "Input Devices" + +config NX_MOUSE + bool "Mouse/Touchscreen Support" + default n + ---help--- + Build in support for mouse or touchscreeninput. + +config NX_KBD + bool "Keyboard Support" + default n + ---help--- + Build in support of keypad/keyboard input. + +endmenu + +menu "Framed Window Borders" + +config NXTK_BORDERWIDTH + int "Border Width" + default 4 + ---help--- + Specifies with with of the border (in pixels) used with framed windows. + The default is 4. + +config NXTK_BORDERCOLOR1 + hex "Border Color" + default 0 + ---help-- + Specify the colors of the border used with framed windows. + NXTL_BODERCOLOR is the "normal" color of the border. + NXTK_BORDERCOLOR2 is the shadow side color and so is normally darker. + NXTK_BORDERCOLOR3 is the shiny side color and so is normally brighter. + +config NXTK_BORDERCOLOR2 + hex "Darker Border Color" + default 0 + ---help-- + Specify the colors of the border used with framed windows. + NXTL_BODERCOLOR is the "normal" color of the border. + NXTK_BORDERCOLOR2 is the shadow side color and so is normally darker. + NXTK_BORDERCOLOR3 is the shiny side color and so is normally brighter. + +config NXTK_BORDERCOLOR3 + hex "Brighter Border Color" + default 0 + ---help-- + Specify the colors of the border used with framed windows. + NXTL_BODERCOLOR is the "normal" color of the border. + NXTK_BORDERCOLOR2 is the shadow side color and so is normally darker. + NXTK_BORDERCOLOR3 is the shiny side color and so is normally brighter. + +endmenu + +config NXTK_AUTORAISE + bool "Autoraise" + default n + ---help--- + If set, a window will be raised to the top if the mouse position is over a + visible portion of the window. Default: A mouse button must be clicked over + a visible portion of the window. + +menu "Font Selections" + +config NXFONTS_CHARBITS + int "Bits in Character Set" + default 7 + range 7 8 + ---help-- + The number of bits in the character set. Current options are only 7 and 8. + The default is 7. + +config NXFONT_SANS17X22 + bool "Sans 17x22" + default n + ---help--- + This option enables support for a tiny, 17x22 san serif font + (font ID FONTID_SANS17X22 == 14). + +config NXFONT_SANS20X26 + bool "Sans 20x26" + default n + ---help--- + This option enables support for a tiny, 20x26 san serif font + (font ID FONTID_SANS20X26 == 15). + +config NXFONT_SANS23X27 + bool "Sans 23x27" + default n + ---help--- + This option enables support for a tiny, 23x27 san serif font + (font ID FONTID_SANS23X27 == 1). + +config NXFONT_SANS22X29 + bool "Sans 22x29" + default n + ---help--- + This option enables support for a small, 22x29 san serif font + (font ID FONTID_SANS22X29 == 2). + +config NXFONT_SANS28X37 + bool "Sans 28x37" + default n + ---help--- + This option enables support for a medium, 28x37 san serif font + (font ID FONTID_SANS28X37 == 3). + +config NXFONT_SANS39X48 + bool "Sans 39x48" + default n + ---help--- + This option enables support for a large, 39x48 san serif font + (font ID FONTID_SANS39X48 == 4). + +config NXFONT_SANS17X23B + bool "Sans 17x23 Bold" + default n + ---help--- + This option enables support for a tiny, 17x23 san serif bold font + (font ID FONTID_SANS17X23B == 16). + +config NXFONT_SANS20X27B + bool "Sans 20x27 Bold" + default n + ---help--- + This option enables support for a tiny, 20x27 san serif bold font + (font ID FONTID_SANS20X27B == 17). + +config NXFONT_SANS22X29B + bool "Sans 22x29 Bold" + default n + ---help--- + This option enables support for a small, 22x29 san serif bold font + (font ID FONTID_SANS22X29B == 5). + +config NXFONT_SANS28X37B + bool "Sans 28x37 Bold" + default n + ---help--- + This option enables support for a medium, 28x37 san serif bold font + (font ID FONTID_SANS28X37B == 6). + +config NXFONT_SANS40X49B + bool "Sans 40x49 Bold" + default n + ---help--- + This option enables support for a large, 40x49 san serif bold font + (font ID FONTID_SANS40X49B == 7). + +config NXFONT_SERIF22X29 + bool "Serif 22x29" + default n + ---help--- + This option enables support for a small, 22x29 font (with serifs) + (font ID FONTID_SERIF22X29 == 8). + +config NXFONT_SERIF29X37 + bool "Serif 29x37" + default n + ---help--- + This option enables support for a medium, 29x37 font (with serifs) + (font ID FONTID_SERIF29X37 == 9). + +config NXFONT_SERIF38X48 + bool "Serif 38x48" + default n + ---help--- + This option enables support for a large, 38x48 font (with serifs) + (font ID FONTID_SERIF38X48 == 10). + +config NXFONT_SERIF22X28B + bool "Serif 22x28 Bold" + default n + ---help--- + This option enables support for a small, 27x38 bold font (with serifs) + (font ID FONTID_SERIF22X28B == 11). + +config NXFONT_SERIF27X38B + bool "Serif 27x38 Bold" + default n + ---help--- + This option enables support for a medium, 27x38 bold font (with serifs) + (font ID FONTID_SERIF27X38B == 12). + +config NXFONT_SERIF38X49B + bool "Serif 38x49 Bold" + default n + ---help--- + This option enables support for a large, 38x49 bold font (with serifs) + (font ID FONTID_SERIF38X49B == 13). + +endmenu + +menuconfig NXCONSOLE + bool "NxConsole" + default n + ---help--- + Enables building of the NxConsole driver. + +if NXCONSOLE + +comment "NxConsole Output Text/Graphics Options" + +config NXCONSOLE_BPP + int "NxConsole BPP" + default 1 if !NX_DISABLE_1BPP + default 2 if !NX_DISABLE_2BPP + default 4 if !NX_DISABLE_4BPP + default 8 if !NX_DISABLE_8BPP + default 16 if !NX_DISABLE_16BPP + default 24 if !NX_DISABLE_24BPP + default 32 if !NX_DISABLE_32BPP + ---help--- + Currently, NxConsole supports only a single pixel depth. This + configuration setting must be provided to support that single pixel depth. + Default: The smallest enabled pixel depth. (see NX_DISABLE_*BPP) + +config NXCONSOLE_CURSORCHAR + int "Character code to use as the cursor" + default 137 + ---help--- + The bitmap code to use as the cursor. Default '_' (137) + +config NXCONSOLE_MXCHARS + int "Max Characters on Display" + default 128 + ---help--- + NxConsole needs to remember every character written to the console so + that it can redraw the window. This setting determines the size of some + internal memory allocations used to hold the character data. Default: 128. + +config NXCONSOLE_CACHESIZE + int "Font Cache Size" + default 16 + ---help--- + NxConsole supports caching of rendered fonts. This font caching is required + for two reasons: (1) First, it improves text performance, but more + importantly (2) it preserves the font memory. Since the NX server runs on + a separate server thread, it requires that the rendered font memory persist + until the server has a chance to render the font. Unfortunately, the font + cache would be quite large if all fonts were saved. The NXCONSOLE_CACHESIZE + setting will control the size of the font cache (in number of glyphs). Only that + number of the most recently used glyphs will be retained. Default: 16. + NOTE: There can still be a race condition between the NxConsole driver and the + NX task. If you every see character corruption (especially when printing + a lot of data or scrolling), then increasing the value of NXCONSOLE_CACHESIZE + is something that you should try. Alternatively, you can reduce the size of + MQ_MAXMSGSIZE which will force NxConsole task to pace the server task. + NXCONSOLE_CACHESIZE should be larger than MQ_MAXMSGSIZE in any event. + +config NXCONSOLE_LINESEPARATION + int "Line Separation + default 0 + ---help--- + This the space (in rows) between each row of test. Default: 0 + +config NXCONSOLE_NOWRAP + bool "No wrap" + default n + ---help--- + By default, lines will wrap when the test reaches the right hand side + of the window. This setting can be defining to change this behavior so + that the text is simply truncated until a new line is encountered. + +commont "NxConsole Input options" + +config NXCONSOLE_NXKBDIN + bool "NX KBD input" + default n + ---help--- + Take input from the NX keyboard input callback. By default, keyboard + input is taken from stdin (/dev/console). If this option is set, then + the interface nxcon_kdbin() is enabled. That interface may be driven + by window callback functions so that keyboard input *only* goes to the + top window. + +config NXCONSOLE_KBDBUFSIZE + int "Keyboard Input Buffer Size" + default 16 + ---help--- + If NXCONSOLE_NXKBDIN is enabled, then this value may be used to + define the size of the per-window keyboard input buffer. Default: 16 + +config NXCONSOLE_NPOLLWAITERS + int "Number of Poll Waiters" + default 4 + ---help--- + The number of threads that can be waiting for read data available. + Default: 4 + +endif +endmenu + +menu "NX Multi-user only options" + +config NX_MULTIUSER + bool "Multi-user NX Server" + default n + ---help--- + Configures NX in multi-user mode + +if NX_MULTIUSER + +config NX_BLOCKING + bool "Blocking" + default n + ---help--- + Open the client message queues in blocking mode. In this case, + nx_eventhandler() will not return until a message is received and processed. + +config NX_MXSERVERMSGS + int "Max Server Messages" + default 32 + ---help--- + Specifies the maximum number of messages that can fit in the message queues. + No additional resources are allocated, but this can be set to prevent + flooding of the client or server with too many messages (PREALLOC_MQ_MSGS + controls how many messages are pre-allocated). + +config NX_MXCLIENTMSGS + int "Max Client Messages" + default 16 + ---help--- + Specifies the maximum number of messages that can fit in the message queues. + No additional resources are allocated, but this can be set to prevent + flooding of the client or server with too many messages (PREALLOC_MQ_MSGS + controls how many messages are pre-allocated). + +endif +endmenu + +endif From 26da04787883aa336e636e4e955f1d4718f02131 Mon Sep 17 00:00:00 2001 From: patacongo Date: Tue, 25 Sep 2012 01:29:27 +0000 Subject: [PATCH 33/56] Hook in graphics/Kconfig git-svn-id: http://svn.code.sf.net/p/nuttx/code/trunk@5187 42af7a65-404d-4744-a932-0658087f49c3 --- nuttx/Kconfig | 4 ++++ nuttx/graphics/Kconfig | 19 ++++++++----------- 2 files changed, 12 insertions(+), 11 deletions(-) diff --git a/nuttx/Kconfig b/nuttx/Kconfig index f920e4984d..520f09ebff 100644 --- a/nuttx/Kconfig +++ b/nuttx/Kconfig @@ -339,6 +339,10 @@ menu "File Systems" source fs/Kconfig endmenu +menu "Graphics Support" +source graphics/Kconfig +endmenu + menu "Memory Management" source mm/Kconfig endmenu diff --git a/nuttx/graphics/Kconfig b/nuttx/graphics/Kconfig index 8af3c2d1ef..1ae387605b 100644 --- a/nuttx/graphics/Kconfig +++ b/nuttx/graphics/Kconfig @@ -124,7 +124,7 @@ config NXTK_BORDERWIDTH config NXTK_BORDERCOLOR1 hex "Border Color" default 0 - ---help-- + ---help--- Specify the colors of the border used with framed windows. NXTL_BODERCOLOR is the "normal" color of the border. NXTK_BORDERCOLOR2 is the shadow side color and so is normally darker. @@ -133,7 +133,7 @@ config NXTK_BORDERCOLOR1 config NXTK_BORDERCOLOR2 hex "Darker Border Color" default 0 - ---help-- + ---help--- Specify the colors of the border used with framed windows. NXTL_BODERCOLOR is the "normal" color of the border. NXTK_BORDERCOLOR2 is the shadow side color and so is normally darker. @@ -142,7 +142,7 @@ config NXTK_BORDERCOLOR2 config NXTK_BORDERCOLOR3 hex "Brighter Border Color" default 0 - ---help-- + ---help--- Specify the colors of the border used with framed windows. NXTL_BODERCOLOR is the "normal" color of the border. NXTK_BORDERCOLOR2 is the shadow side color and so is normally darker. @@ -164,7 +164,7 @@ config NXFONTS_CHARBITS int "Bits in Character Set" default 7 range 7 8 - ---help-- + ---help--- The number of bits in the character set. Current options are only 7 and 8. The default is 7. @@ -347,7 +347,7 @@ config NXCONSOLE_CACHESIZE NXCONSOLE_CACHESIZE should be larger than MQ_MAXMSGSIZE in any event. config NXCONSOLE_LINESEPARATION - int "Line Separation + int "Line Separation" default 0 ---help--- This the space (in rows) between each row of test. Default: 0 @@ -360,7 +360,7 @@ config NXCONSOLE_NOWRAP of the window. This setting can be defining to change this behavior so that the text is simply truncated until a new line is encountered. -commont "NxConsole Input options" +comment "NxConsole Input options" config NXCONSOLE_NXKBDIN bool "NX KBD input" @@ -387,11 +387,10 @@ config NXCONSOLE_NPOLLWAITERS Default: 4 endif -endmenu -menu "NX Multi-user only options" +comment "NX Multi-user only options" -config NX_MULTIUSER +menuconfig NX_MULTIUSER bool "Multi-user NX Server" default n ---help--- @@ -425,6 +424,4 @@ config NX_MXCLIENTMSGS controls how many messages are pre-allocated). endif -endmenu - endif From 6b0ed71ae02a56692a80e47bd11baa7e14d9284e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 25 Sep 2012 16:36:33 +0200 Subject: [PATCH 34/56] Simplified magnetometer calibration routine --- apps/commander/commander.c | 179 +++++++++-------------------------- apps/systemlib/param/param.c | 4 +- 2 files changed, 49 insertions(+), 134 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 7535b90469..4c20a2f71e 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -307,30 +307,10 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) const uint64_t calibration_interval_us = 45 * 1000000; unsigned int calibration_counter = 0; - const int peak_samples = 2000; - /* Get rid of 10% */ - const int outlier_margin = (peak_samples) / 10; + const int peak_samples = 500; - float *mag_maxima[3]; - mag_maxima[0] = (float*)malloc(peak_samples * sizeof(float)); - mag_maxima[1] = (float*)malloc(peak_samples * sizeof(float)); - mag_maxima[2] = (float*)malloc(peak_samples * sizeof(float)); - - float *mag_minima[3]; - mag_minima[0] = (float*)malloc(peak_samples * sizeof(float)); - mag_minima[1] = (float*)malloc(peak_samples * sizeof(float)); - mag_minima[2] = (float*)malloc(peak_samples * sizeof(float)); - - /* initialize data table */ - for (int i = 0; i < peak_samples; i++) { - mag_maxima[0][i] = FLT_MIN; - mag_maxima[1][i] = FLT_MIN; - mag_maxima[2][i] = FLT_MIN; - - mag_minima[0][i] = FLT_MAX; - mag_minima[1][i] = FLT_MAX; - mag_minima[2][i] = FLT_MAX; - } + float mag_max[3] = {0, 0, 0}; + float mag_min[3] = {0, 0, 0}; int fd = open(MAG_DEVICE_PATH, 0); struct mag_scale mscale_null = { @@ -357,27 +337,25 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); /* get min/max values */ - /* iterate through full list */ - for (int i = 0; i < peak_samples; i++) { - /* x minimum */ - if (raw.magnetometer_raw[0] < mag_minima[0][i]) - mag_minima[0][i] = raw.magnetometer_ga[0]; - /* y minimum */ - if (raw.magnetometer_raw[1] < mag_minima[1][i]) - mag_minima[1][i] = raw.magnetometer_ga[1]; - /* z minimum */ - if (raw.magnetometer_raw[2] < mag_minima[2][i]) - mag_minima[2][i] = raw.magnetometer_ga[2]; + if (raw.magnetometer_raw[0] < mag_min[0]) { + mag_min[0] = raw.magnetometer_raw[0]; + } + else if (raw.magnetometer_raw[0] > mag_max[0]) { + mag_max[0] = raw.magnetometer_raw[0]; + } - /* x maximum */ - if (raw.magnetometer_raw[0] > mag_maxima[0][i]) - mag_maxima[0][i] = raw.magnetometer_ga[0]; - /* y maximum */ - if (raw.magnetometer_raw[1] > mag_maxima[1][i]) - mag_maxima[1][i] = raw.magnetometer_ga[1]; - /* z maximum */ - if (raw.magnetometer_raw[2] > mag_maxima[2][i]) - mag_maxima[2][i] = raw.magnetometer_ga[2]; + if (raw.magnetometer_raw[1] < mag_min[1]) { + mag_min[1] = raw.magnetometer_raw[1]; + } + else if (raw.magnetometer_raw[1] > mag_max[1]) { + mag_max[1] = raw.magnetometer_raw[1]; + } + + if (raw.magnetometer_raw[2] < mag_min[2]) { + mag_min[2] = raw.magnetometer_raw[2]; + } + else if (raw.magnetometer_raw[2] > mag_max[2]) { + mag_max[2] = raw.magnetometer_raw[2]; } calibration_counter++; @@ -392,67 +370,6 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) status->flag_preflight_mag_calibration = false; state_machine_publish(status_pub, status, mavlink_fd); - /* sort values */ - cal_bsort(mag_minima[0], peak_samples); - cal_bsort(mag_minima[1], peak_samples); - cal_bsort(mag_minima[2], peak_samples); - - cal_bsort(mag_maxima[0], peak_samples); - cal_bsort(mag_maxima[1], peak_samples); - cal_bsort(mag_maxima[2], peak_samples); - - float min_avg[3] = { 0.0f, 0.0f, 0.0f }; - float max_avg[3] = { 0.0f, 0.0f, 0.0f }; - - // printf("start:\n"); - - // for (int i = 0; i < 10; i++) { - // printf("mag min: %8.4f\t%8.4f\t%8.4f\tmax: %8.4f\t%8.4f\t%8.4f\n", - // mag_minima[0][i], - // mag_minima[1][i], - // mag_minima[2][i], - // mag_maxima[0][i], - // mag_maxima[1][i], - // mag_maxima[2][i]); - // usleep(10000); - // } - // printf("-----\n"); - - // for (int i = (peak_samples - outlier_margin)-10; i < (peak_samples - outlier_margin); i++) { - // printf("mag min: %8.4f\t%8.4f\t%8.4f\tmax: %8.4f\t%8.4f\t%8.4f\n", - // mag_minima[0][i], - // mag_minima[1][i], - // mag_minima[2][i], - // mag_maxima[0][i], - // mag_maxima[1][i], - // mag_maxima[2][i]); - // usleep(10000); - // } - - // printf("end\n"); - - /* take average of center value group */ - for (int i = 0; i < (peak_samples - outlier_margin); i++) { - min_avg[0] += mag_minima[0][i+outlier_margin]; - min_avg[1] += mag_minima[1][i+outlier_margin]; - min_avg[2] += mag_minima[2][i+outlier_margin]; - - max_avg[0] += mag_maxima[0][i]; - max_avg[1] += mag_maxima[1][i]; - max_avg[2] += mag_maxima[2][i]; - } - - min_avg[0] /= (peak_samples - outlier_margin); - min_avg[1] /= (peak_samples - outlier_margin); - min_avg[2] /= (peak_samples - outlier_margin); - - max_avg[0] /= (peak_samples - outlier_margin); - max_avg[1] /= (peak_samples - outlier_margin); - max_avg[2] /= (peak_samples - outlier_margin); - - // printf("\nFINAL:\nmag min: %8.4f\t%8.4f\t%8.4f\nmax: %8.4f\t%8.4f\t%8.4f\n", (double)min_avg[0], - // (double)min_avg[1], (double)min_avg[2], (double)max_avg[0], (double)max_avg[1], (double)max_avg[2]); - float mag_offset[3]; /** @@ -467,31 +384,37 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) * offset = (max + min) / 2.0f */ - mag_offset[0] = (max_avg[0] + min_avg[0]) / 2.0f; - mag_offset[1] = (max_avg[1] + min_avg[1]) / 2.0f; - mag_offset[2] = (max_avg[2] + min_avg[2]) / 2.0f; + printf("max 0: %f\n",mag_max[0]); + printf("max 1: %f\n",mag_max[1]); + printf("max 2: %f\n",mag_max[2]); + printf("min 0: %f\n",mag_min[0]); + printf("min 1: %f\n",mag_min[1]); + printf("min 2: %f\n",mag_min[2]); - if (!isfinite(mag_offset[1]) || !isfinite(mag_offset[1]) || !isfinite(mag_offset[2])) { + mag_offset[0] = (mag_max[0] + mag_min[0]) / 2.0f; + mag_offset[1] = (mag_max[1] + mag_min[1]) / 2.0f; + mag_offset[2] = (mag_max[2] + mag_min[2]) / 2.0f; - mavlink_log_critical(mavlink_fd, "[commander] MAG calibration failed (INF/NAN)"); - } else { - /* announce and set new offset */ + printf("mag off 0: %f\n",mag_offset[0]); + printf("mag off 1: %f\n",mag_offset[1]); + printf("mag off 2: %f\n",mag_offset[2]); - // char offset_output[50]; - // sprintf(offset_output, "[commander] mag cal: %8.4f %8.4f %8.4f", (double)mag_offset[0], (double)mag_offset[1], (double)mag_offset[2]); - // mavlink_log_info(mavlink_fd, offset_output); + /* announce and set new offset */ - if (param_set(param_find("SENS_MAG_XOFF"), &(mag_offset[0]))) { - fprintf(stderr, "[commander] Setting X mag offset failed!\n"); - } - - if (param_set(param_find("SENS_MAG_YOFF"), &(mag_offset[1]))) { - fprintf(stderr, "[commander] Setting Y mag offset failed!\n"); - } + // char offset_output[50]; + // sprintf(offset_output, "[commander] mag cal: %8.4f %8.4f %8.4f", (double)mag_offset[0], (double)mag_offset[1], (double)mag_offset[2]); + // mavlink_log_info(mavlink_fd, offset_output); - if (param_set(param_find("SENS_MAG_ZOFF"), &(mag_offset[2]))) { - fprintf(stderr, "[commander] Setting Z mag offset failed!\n"); - } + if (param_set(param_find("SENS_MAG_XOFF"), &(mag_offset[0]))) { + fprintf(stderr, "[commander] Setting X mag offset failed!\n"); + } + + if (param_set(param_find("SENS_MAG_YOFF"), &(mag_offset[1]))) { + fprintf(stderr, "[commander] Setting Y mag offset failed!\n"); + } + + if (param_set(param_find("SENS_MAG_ZOFF"), &(mag_offset[2]))) { + fprintf(stderr, "[commander] Setting Z mag offset failed!\n"); } fd = open(MAG_DEVICE_PATH, 0); @@ -507,14 +430,6 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) warn("WARNING: failed to set scale / offsets for mag"); close(fd); - free(mag_maxima[0]); - free(mag_maxima[1]); - free(mag_maxima[2]); - - free(mag_minima[0]); - free(mag_minima[1]); - free(mag_minima[2]); - /* auto-save to EEPROM */ int save_ret = pm_save_eeprom(false); if(save_ret != 0) { diff --git a/apps/systemlib/param/param.c b/apps/systemlib/param/param.c index a01c170a68..0ab7c0ea30 100644 --- a/apps/systemlib/param/param.c +++ b/apps/systemlib/param/param.c @@ -484,7 +484,7 @@ param_export(int fd, bool only_unsaved) s->unsaved = false; - /* append the appripriate BSON type object */ + /* append the appropriate BSON type object */ switch (param_type(s->param)) { case PARAM_TYPE_INT32: param_get(s->param, &i); @@ -688,4 +688,4 @@ param_foreach(void (*func)(void *arg, param_t param), void *arg, bool only_chang func(arg, param); } -} \ No newline at end of file +} From 5c00ca343fabce95273d15b13da4460935134fd3 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 25 Sep 2012 16:39:15 +0200 Subject: [PATCH 35/56] forgot to remove printfs of magnetometer calibration --- apps/commander/commander.c | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 4c20a2f71e..3ac01ee1bd 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -384,20 +384,20 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) * offset = (max + min) / 2.0f */ - printf("max 0: %f\n",mag_max[0]); - printf("max 1: %f\n",mag_max[1]); - printf("max 2: %f\n",mag_max[2]); - printf("min 0: %f\n",mag_min[0]); - printf("min 1: %f\n",mag_min[1]); - printf("min 2: %f\n",mag_min[2]); +// printf("max 0: %f\n",mag_max[0]); +// printf("max 1: %f\n",mag_max[1]); +// printf("max 2: %f\n",mag_max[2]); +// printf("min 0: %f\n",mag_min[0]); +// printf("min 1: %f\n",mag_min[1]); +// printf("min 2: %f\n",mag_min[2]); mag_offset[0] = (mag_max[0] + mag_min[0]) / 2.0f; mag_offset[1] = (mag_max[1] + mag_min[1]) / 2.0f; mag_offset[2] = (mag_max[2] + mag_min[2]) / 2.0f; - printf("mag off 0: %f\n",mag_offset[0]); - printf("mag off 1: %f\n",mag_offset[1]); - printf("mag off 2: %f\n",mag_offset[2]); +// printf("mag off 0: %f\n",mag_offset[0]); +// printf("mag off 1: %f\n",mag_offset[1]); +// printf("mag off 2: %f\n",mag_offset[2]); /* announce and set new offset */ From 734ea8dc168c8b412af2548cdc27d7fa02cd4f7f Mon Sep 17 00:00:00 2001 From: patacongo Date: Tue, 25 Sep 2012 19:17:52 +0000 Subject: [PATCH 36/56] Port STM3240G-EVAL ILI93xx driver to Shenzhou board git-svn-id: http://svn.code.sf.net/p/nuttx/code/trunk@5188 42af7a65-404d-4744-a932-0658087f49c3 --- .../configs/olimex-stm32-p107/include/board.h | 27 +- nuttx/configs/shenzhou/include/board.h | 38 + nuttx/configs/shenzhou/src/Makefile | 6 +- .../configs/shenzhou/src/shenzhou-internal.h | 56 +- nuttx/configs/shenzhou/src/up_lcd.c | 1940 +++++++++++++++++ nuttx/configs/shenzhou/src/up_ssd1289.c | 124 +- 6 files changed, 2157 insertions(+), 34 deletions(-) create mode 100644 nuttx/configs/shenzhou/src/up_lcd.c diff --git a/nuttx/configs/olimex-stm32-p107/include/board.h b/nuttx/configs/olimex-stm32-p107/include/board.h index 42dd4f4d5b..ce0c82472f 100644 --- a/nuttx/configs/olimex-stm32-p107/include/board.h +++ b/nuttx/configs/olimex-stm32-p107/include/board.h @@ -33,6 +33,9 @@ * ************************************************************************************/ +#ifndef __CONFIGS_OLIMEX_STM32_P107_INCLUDE_BOARD_H +#define __CONFIGS_OLIMEX_STM32_P107_INCLUDE_BOARD_H 1 + /************************************************************************************ * Included Files ************************************************************************************/ @@ -105,6 +108,20 @@ # define STM32_PLL_PLL3MUL RCC_CFGR2_PLL3MULx10 /* MCO 5MHz * 10 = 50MHz */ #endif +/************************************************************************************ + * Public Data + ************************************************************************************/ + +#ifndef __ASSEMBLY__ + +#undef EXTERN +#if defined(__cplusplus) +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + /************************************************************************************ * Public Function Prototypes ************************************************************************************/ @@ -118,4 +135,12 @@ * ************************************************************************************/ -void stm32_boardinitialize(void); +EXTERN void stm32_boardinitialize(void); + +#undef EXTERN +#if defined(__cplusplus) +} +#endif + +#endif /* __ASSEMBLY__ */ +#endif /* __CONFIGS_OLIMEX_STM32_P107_INCLUDE_BOARD_H */ diff --git a/nuttx/configs/shenzhou/include/board.h b/nuttx/configs/shenzhou/include/board.h index d396e25631..c105d4ab5e 100644 --- a/nuttx/configs/shenzhou/include/board.h +++ b/nuttx/configs/shenzhou/include/board.h @@ -33,6 +33,9 @@ * ************************************************************************************/ +#ifndef __CONFIGS_SHENZHOU_INCLUDE_BOARD_H +#define __CONFIGS_SHENZHOU_INCLUDE_BOARD_H 1 + /************************************************************************************ * Included Files ************************************************************************************/ @@ -321,6 +324,20 @@ * 63 PC6 I2S_MCK GPIO_I2S2_MCK. Audio DAC. Active low: Pulled high */ +/************************************************************************************ + * Public Data + ************************************************************************************/ + +#ifndef __ASSEMBLY__ + +#undef EXTERN +#if defined(__cplusplus) +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + /************************************************************************************ * Public Function Prototypes ************************************************************************************/ @@ -382,3 +399,24 @@ EXTERN void stm32_ledinit(void); EXTERN void stm32_setled(int led, bool ledon); EXTERN void stm32_setleds(uint8_t ledset); #endif + +/************************************************************************************ + * Name: stm32_lcdclear + * + * Description: + * This is a non-standard LCD interface just for the Shenzhou board. Because + * of the various rotations, clearing the display in the normal way by writing a + * sequences of runs that covers the entire display can be very slow. Here the + * display is cleared by simply setting all GRAM memory to the specified color. + * + ************************************************************************************/ + +EXTERN void stm32_lcdclear(uint16_t color); + +#undef EXTERN +#if defined(__cplusplus) +} +#endif + +#endif /* __ASSEMBLY__ */ +#endif /* __CONFIGS_SHENZHOU_INCLUDE_BOARD_H */ diff --git a/nuttx/configs/shenzhou/src/Makefile b/nuttx/configs/shenzhou/src/Makefile index 2bfec8072c..fc1d6caa1f 100644 --- a/nuttx/configs/shenzhou/src/Makefile +++ b/nuttx/configs/shenzhou/src/Makefile @@ -84,8 +84,12 @@ ifeq ($(CONFIG_WATCHDOG),y) CSRCS += up_watchdog.c endif +# NOTE: SSD1289 is not supported on the board + ifeq ($(CONFIG_LCD_SSD1289),y) -CSRCS += up_ssd1289.c +CSRCS += up_ssd1289.c +else +CSRCS += up_lcd.c endif COBJS = $(CSRCS:.c=$(OBJEXT)) diff --git a/nuttx/configs/shenzhou/src/shenzhou-internal.h b/nuttx/configs/shenzhou/src/shenzhou-internal.h index 0ac88c4578..095cba4850 100644 --- a/nuttx/configs/shenzhou/src/shenzhou-internal.h +++ b/nuttx/configs/shenzhou/src/shenzhou-internal.h @@ -224,39 +224,56 @@ /* TFT LCD GPIOs */ -#define GPIO_LCD_D0 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ +#define GPIO_LCD_D0OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN0) -#define GPIO_LCD_D1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ +#define GPIO_LCD_D1OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN1) -#define GPIO_LCD_D2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ +#define GPIO_LCD_D2OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN2) -#define GPIO_LCD_D3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ +#define GPIO_LCD_D3OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3) -#define GPIO_LCD_D4 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ +#define GPIO_LCD_D4OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN4) -#define GPIO_LCD_D5 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ +#define GPIO_LCD_D5OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN5) -#define GPIO_LCD_D6 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ +#define GPIO_LCD_D6OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN6) -#define GPIO_LCD_D7 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ +#define GPIO_LCD_D7OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN7) -#define GPIO_LCD_D8 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ +#define GPIO_LCD_D8OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN8) -#define GPIO_LCD_D9 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ +#define GPIO_LCD_D9OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN9) -#define GPIO_LCD_D10 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ +#define GPIO_LCD_D10OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN10) -#define GPIO_LCD_D11 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ +#define GPIO_LCD_D11OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN11) -#define GPIO_LCD_D12 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ +#define GPIO_LCD_D12OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN12) -#define GPIO_LCD_D13 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ +#define GPIO_LCD_D13OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN13) -#define GPIO_LCD_D14 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ +#define GPIO_LCD_D14OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN14) -#define GPIO_LCD_D15 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ +#define GPIO_LCD_D15OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN15) +#define GPIO_LCD_D0IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_PORTE|GPIO_PIN0) +#define GPIO_LCD_D1IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_PORTE|GPIO_PIN1) +#define GPIO_LCD_D2IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_PORTE|GPIO_PIN2) +#define GPIO_LCD_D3IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_PORTE|GPIO_PIN3) +#define GPIO_LCD_D4IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_PORTE|GPIO_PIN4) +#define GPIO_LCD_D5IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_PORTE|GPIO_PIN5) +#define GPIO_LCD_D6IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_PORTE|GPIO_PIN6) +#define GPIO_LCD_D7IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_PORTE|GPIO_PIN7) +#define GPIO_LCD_D8IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_PORTE|GPIO_PIN8) +#define GPIO_LCD_D9IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_PORTE|GPIO_PIN9) +#define GPIO_LCD_D10IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_PORTE|GPIO_PIN10) +#define GPIO_LCD_D11IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_PORTE|GPIO_PIN11) +#define GPIO_LCD_D12IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_PORTE|GPIO_PIN12) +#define GPIO_LCD_D13IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_PORTE|GPIO_PIN13) +#define GPIO_LCD_D14IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_PORTE|GPIO_PIN14) +#define GPIO_LCD_D15IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_PORTE|GPIO_PIN15) + #define GPIO_LCD_RS (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN13) #define GPIO_LCD_CS (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ @@ -290,7 +307,12 @@ #define LCD_LE_CLEAR LCD_BIT_CLEAR(STM32_GPIOB_OFFSET, 2) /* GPIO_PORTB|GPIO_PIN2 */ #define LCD_LE_SET LCD_BIT_SET(STM32_GPIOB_OFFSET, 2) -#define LCD_DATA STM32_GPIOE_ODR +#define LCD_CRL STM32_GPIOE_CRL +#define LCD_CRH STM32_GPIOE_CRH +#define LCD_INPUT 0x44444444 +#define LCD_OUTPUT 0x33333333 +#define LCD_ODR STM32_GPIOE_ODR +#define LCD_IDR STM32_GPIOE_IDR /* Touchscreen IC on the LCD module */ diff --git a/nuttx/configs/shenzhou/src/up_lcd.c b/nuttx/configs/shenzhou/src/up_lcd.c new file mode 100644 index 0000000000..a58b795c42 --- /dev/null +++ b/nuttx/configs/shenzhou/src/up_lcd.c @@ -0,0 +1,1940 @@ +/************************************************************************************ + * configs/shenzhou/src/up_lcd.c + * arch/arm/src/board/up_lcd.c + * + * Copyright (C) 2012 Gregory Nutt. All rights reserved. + * Authors: Gregory Nutt + * Diego Sanchez + * + * 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. + * + ************************************************************************************/ +/* TFT LCD + * + * -- ---- -------------- ----------------------------------------------------------- + * PN NAME SIGNAL NOTES + * -- ---- -------------- ----------------------------------------------------------- + * 37 PB2 DATA_LE To TFT LCD (CN13, ping 28) + * 96 PB9 F_CS To both the TFT LCD (CN13, pin 30) and to the W25X16 SPI FLASH + * 34 PC5 TP_INT JP6. To TFT LCD (CN13) module (CN13, pin 26) + * 65 PC8 LCD_CS Active low: Pulled high (CN13, pin 19) + * 66 PC9 TP_CS Active low: Pulled high (CN13, pin 31) + * 78 PC10 SPI3_SCK To TFT LCD (CN13, pin 29) + * 79 PC11 SPI3_MISO To TFT LCD (CN13, pin 25) + * 80 PC12 SPI3_MOSI To TFT LCD (CN13, pin 27) + * 58 PD11 SD_CS Active low: Pulled high (See also TFT LCD CN13, pin 32) + * 60 PD13 LCD_RS To TFT LCD (CN13, pin 20) + * 61 PD14 LCD_WR To TFT LCD (CN13, pin 21) + * 62 PD15 LCD_RD To TFT LCD (CN13, pin 22) + * 97 PE0 DB00 To TFT LCD (CN13, pin 3) + * 98 PE1 DB01 To TFT LCD (CN13, pin 4) + * 1 PE2 DB02 To TFT LCD (CN13, pin 5) + * 2 PE3 DB03 To TFT LCD (CN13, pin 6) + * 3 PE4 DB04 To TFT LCD (CN13, pin 7) + * 4 PE5 DB05 To TFT LCD (CN13, pin 8) + * 5 PE6 DB06 To TFT LCD (CN13, pin 9) + * 38 PE7 DB07 To TFT LCD (CN13, pin 10) + * 39 PE8 DB08 To TFT LCD (CN13, pin 11) + * 40 PE9 DB09 To TFT LCD (CN13, pin 12) + * 41 PE10 DB10 To TFT LCD (CN13, pin 13) + * 42 PE11 DB11 To TFT LCD (CN13, pin 16) + * 43 PE12 DB12 To TFT LCD (CN13, pin 15) + * 44 PE13 DB13 To TFT LCD (CN13, pin 16) + * 45 PE14 DB14 To TFT LCD (CN13, pin 17) + * 46 PE15 DB15 To TFT LCD (CN13, pin 18) + * + * NOTE: The backlight signl NC_BL (CN13, pin 24) is pulled high and not under + * software control + * + * On LCD module: + * -- -------------- ------------------------------------------------------------------- + * PN SIGNAL NOTES + * -- -------------- ------------------------------------------------------------------- + * 3 DB01 To LCD DB1 + * 4 DB00 To LCD DB0 + * 5 DB03 To LCD DB3 + * 6 DB02 To LCD DB2 + * 7 DB05 To LCD DB5 + * 8 DB04 To LCD DB4 + * 9 DB07 To LCD DB7 + * 10 DB06 To LCD DB6 + * 11 DB09 To LCD DB9 + * 12 DB08 To LCD DB8 + * 13 DB11 To LCD DB11 + * 14 DB10 To LCD DB10 + * 15 DB13 To LCD DB13 + * 16 DB12 To LCD DB12 + * 17 DB15 To LCD DB15 + * 18 DB14 To LCD DB14 + * 19 RS To LCD RS + * 20 /LCD_CS To LCD CS + * 21 /RD To LCD RD + * 22 /WR To LCD WR + * 23 BL_EN (Not referenced) + * 24 /RESET + * 25 /INT To Touch IC /INT + * 26 MISO To Touch IC DOUT; To AT45DB161B SO; To SD card DAT0 + * 27 LE To 74HC573 that controls LCD 8-bit/16-bit mode + * 28 MOSI To Touch IC DIN; To AT45DB161B SI; To SD card CMD + * 29 /DF_CS To AT45DB161B Data Flash /CS + * 30 SCLK To Touch IC DCLK; To AT45DB161B SCK; To SD card CLK + * 31 /SD_CS To SD card /CS + * 31 /TP_CS To Touch IC CS + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include "up_arch.h" +#include "stm32.h" +#include "stm32_internal.h" +#include "shenzhou-internal.h" + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ +/* Configuration **********************************************************************/ +/* + * CONFIG_STM32_ILI1505_DISABLE may be defined to disable the LCD_ILI1505 + * CONFIG_STM32_ILI9300_DISABLE may be defined to disable the LCD_ILI9300 + * CONFIG_STM32_ILI9320_DISABLE may be defined to disable the LCD_ILI9320 + * CONFIG_STM32_ILI9321_DISABLE may be defined to disable the LCD_ILI9321 + * CONFIG_STM32_ILI9325_DISABLE may be defined to disabled the LCD_ILI9325 + * CONFIG_STM32_ILI9328_DISABLE may be defined to disabled the LCD_ILI9328 + * CONFIG_STM32_ILI9331_DISABLE may be defined to disabled the LCD_ILI9331 + * CONFIG_STM32_ILI9919_DISABLE may be defined to disabled the LCD_ILI9919 + */ + +#undef HAVE_LCD +#if !defined(CONFIG_STM32_ILI1505_DISABLE) +# define HAVE_LCD 1 +#elif !defined(CONFIG_STM32_ILI9300_DISABLE) +# define HAVE_LCD 1 +#elif !defined(CONFIG_STM32_ILI9320_DISABLE) +# define HAVE_LCD 1 +#elif !defined(CONFIG_STM32_ILI9321_DISABLE) +# define HAVE_LCD 1 +#elif !defined(CONFIG_STM32_ILI9325_DISABLE) +# define HAVE_LCD 1 +#elif !defined(CONFIG_STM32_ILI9328_DISABLE) +# define HAVE_LCD 1 +#elif !defined(CONFIG_STM32_ILI9331_DISABLE) +# define HAVE_LCD 1 +#elif !defined(CONFIG_STM32_ILI9919_DISABLE) +# define HAVE_LCD 1 +#endif + +#ifdef HAVE_LCD + +/* Check contrast selection */ + +#if !defined(CONFIG_LCD_MAXCONTRAST) +# define CONFIG_LCD_MAXCONTRAST 1 +#endif + +/* Check power setting */ + +#if !defined(CONFIG_LCD_MAXPOWER) || CONFIG_LCD_MAXPOWER < 1 +# define CONFIG_LCD_MAXPOWER 1 +#endif + +#if CONFIG_LCD_MAXPOWER > 255 +# error "CONFIG_LCD_MAXPOWER must be less than 256 to fit in uint8_t" +#endif + +/* Check orientation */ + +#if defined(CONFIG_LCD_PORTRAIT) +# if defined(CONFIG_LCD_LANDSCAPE) || defined(CONFIG_LCD_RLANDSCAPE) || defined(CONFIG_LCD_RPORTRAIT) +# error "Cannot define both portrait and any other orientations" +# endif +#elif defined(CONFIG_LCD_RPORTRAIT) +# if defined(CONFIG_LCD_LANDSCAPE) || defined(CONFIG_LCD_RLANDSCAPE) +# error "Cannot define both rportrait and any other orientations" +# endif +#elif defined(CONFIG_LCD_LANDSCAPE) +# ifdef CONFIG_LCD_RLANDSCAPE +# error "Cannot define both landscape and any other orientations" +# endif +#elif !defined(CONFIG_LCD_RLANDSCAPE) +# define CONFIG_LCD_LANDSCAPE 1 +#endif + +#undef CONFIG_LCD_FASTCONFIG +#define CONFIG_LCD_FASTCONFIG 1 + +/* Define CONFIG_DEBUG_LCD to enable detailed LCD debug output. Verbose debug must + * also be enabled. + */ + +#ifndef CONFIG_DEBUG +# undef CONFIG_DEBUG_VERBOSE +# undef CONFIG_DEBUG_GRAPHICS +# undef CONFIG_DEBUG_LCD +#endif + +#ifndef CONFIG_DEBUG_VERBOSE +# undef CONFIG_DEBUG_LCD +#endif + +/* Display/Color Properties ***********************************************************/ +/* Display Resolution */ + +#if defined(CONFIG_LCD_LANDSCAPE) || defined(CONFIG_LCD_RLANDSCAPE) +# define STM32_XRES 320 +# define STM32_YRES 240 +#else +# define STM32_XRES 240 +# define STM32_YRES 320 +#endif + +/* Color depth and format */ + +#define STM32_BPP 16 +#define STM32_COLORFMT FB_FMT_RGB16_565 + +/* Shenzhou LCD Hardware Definitions **************************************************/ +/* LCD /CS is CE4, Bank 3 of NOR/SRAM Bank 1~4 */ + +#define STM32_LCDBASE ((uintptr_t)(0x60000000 | 0x08000000)) +#define LCD ((struct lcd_regs_s *)STM32_LCDBASE) + +#define LCD_REG_0 0x00 +#define LCD_REG_1 0x01 +#define LCD_REG_2 0x02 +#define LCD_REG_3 0x03 +#define LCD_REG_4 0x04 +#define LCD_REG_5 0x05 +#define LCD_REG_6 0x06 +#define LCD_REG_7 0x07 +#define LCD_REG_8 0x08 +#define LCD_REG_9 0x09 +#define LCD_REG_10 0x0a +#define LCD_REG_11 0x0b +#define LCD_REG_12 0x0c +#define LCD_REG_13 0x0d +#define LCD_REG_14 0x0e +#define LCD_REG_15 0x0f +#define LCD_REG_16 0x10 +#define LCD_REG_17 0x11 +#define LCD_REG_18 0x12 +#define LCD_REG_19 0x13 +#define LCD_REG_20 0x14 +#define LCD_REG_21 0x15 +#define LCD_REG_22 0x16 +#define LCD_REG_23 0x17 +#define LCD_REG_24 0x18 +#define LCD_REG_25 0x19 +#define LCD_REG_26 0x1a +#define LCD_REG_27 0x1b +#define LCD_REG_28 0x1c +#define LCD_REG_29 0x1d +#define LCD_REG_30 0x1e +#define LCD_REG_31 0x1f +#define LCD_REG_32 0x20 +#define LCD_REG_33 0x21 +#define LCD_REG_34 0x22 +#define LCD_REG_36 0x24 +#define LCD_REG_37 0x25 +#define LCD_REG_38 0x26 +#define LCD_REG_39 0x27 +#define LCD_REG_40 0x28 +#define LCD_REG_41 0x29 +#define LCD_REG_42 0x2a +#define LCD_REG_43 0x2b +#define LCD_REG_45 0x2d +#define LCD_REG_48 0x30 +#define LCD_REG_49 0x31 +#define LCD_REG_50 0x32 +#define LCD_REG_51 0x33 +#define LCD_REG_52 0x34 +#define LCD_REG_53 0x35 +#define LCD_REG_54 0x36 +#define LCD_REG_55 0x37 +#define LCD_REG_56 0x38 +#define LCD_REG_57 0x39 +#define LCD_REG_58 0x3a +#define LCD_REG_59 0x3b +#define LCD_REG_60 0x3c +#define LCD_REG_61 0x3d +#define LCD_REG_62 0x3e +#define LCD_REG_63 0x3f +#define LCD_REG_64 0x40 +#define LCD_REG_65 0x41 +#define LCD_REG_66 0x42 +#define LCD_REG_67 0x43 +#define LCD_REG_68 0x44 +#define LCD_REG_69 0x45 +#define LCD_REG_70 0x46 +#define LCD_REG_71 0x47 +#define LCD_REG_72 0x48 +#define LCD_REG_73 0x49 +#define LCD_REG_74 0x4a +#define LCD_REG_75 0x4b +#define LCD_REG_76 0x4c +#define LCD_REG_77 0x4d +#define LCD_REG_78 0x4e +#define LCD_REG_79 0x4f +#define LCD_REG_80 0x50 +#define LCD_REG_81 0x51 +#define LCD_REG_82 0x52 +#define LCD_REG_83 0x53 +#define LCD_REG_96 0x60 +#define LCD_REG_97 0x61 +#define LCD_REG_106 0x6a +#define LCD_REG_118 0x76 +#define LCD_REG_128 0x80 +#define LCD_REG_129 0x81 +#define LCD_REG_130 0x82 +#define LCD_REG_131 0x83 +#define LCD_REG_132 0x84 +#define LCD_REG_133 0x85 +#define LCD_REG_134 0x86 +#define LCD_REG_135 0x87 +#define LCD_REG_136 0x88 +#define LCD_REG_137 0x89 +#define LCD_REG_139 0x8b +#define LCD_REG_140 0x8c +#define LCD_REG_141 0x8d +#define LCD_REG_143 0x8f +#define LCD_REG_144 0x90 +#define LCD_REG_145 0x91 +#define LCD_REG_146 0x92 +#define LCD_REG_147 0x93 +#define LCD_REG_148 0x94 +#define LCD_REG_149 0x95 +#define LCD_REG_150 0x96 +#define LCD_REG_151 0x97 +#define LCD_REG_152 0x98 +#define LCD_REG_153 0x99 +#define LCD_REG_154 0x9a +#define LCD_REG_157 0x9d +#define LCD_REG_164 0xa4 +#define LCD_REG_192 0xc0 +#define LCD_REG_193 0xc1 +#define LCD_REG_227 0xe3 +#define LCD_REG_229 0xe5 +#define LCD_REG_231 0xe7 +#define LCD_REG_239 0xef + +/* LCD IDs */ + +#define ILI1505_ID 0x1505 +#define ILI9300_ID 0x9300 +#define ILI9320_ID 0x9320 +#define ILI9321_ID 0x9321 +#define ILI9325_ID 0x9325 +#define ILI9328_ID 0x9328 +#define ILI9331_ID 0x9331 +#define ILI9919_ID 0x9919 + +/* Debug ******************************************************************************/ + +#ifdef CONFIG_DEBUG_LCD +# define lcddbg dbg +# define lcdvdbg vdbg +#else +# define lcddbg(x...) +# define lcdvdbg(x...) +#endif + +/************************************************************************************ + * Private Type Definition + ************************************************************************************/ + +/* LCD type */ + +enum lcd_type_e +{ + LCD_TYPE_UNKNOWN = 0, + LCD_TYPE_ILI1505, + LCD_TYPE_ILI9300, + LCD_TYPE_ILI9320, + LCD_TYPE_ILI9321, + LCD_TYPE_ILI9325, + LCD_TYPE_ILI9328, + LCD_TYPE_ILI9331, + LCD_TYPE_ILI9919 +}; + +/* This structure describes the LCD registers */ + +struct lcd_regs_s +{ + volatile uint16_t address; + volatile uint16_t value; +}; + +/* This structure describes the state of this driver */ + +struct stm32_dev_s +{ + /* Publically visible device structure */ + + struct lcd_dev_s dev; + + /* Private LCD-specific information follows */ + + uint8_t type; /* LCD type. See enum lcd_type_e */ + uint8_t power; /* Current power setting */ + bool output; /* True: Configured for output */ +}; + +/************************************************************************************ + * Private Function Protototypes + ************************************************************************************/ +/* Low Level LCD access */ + +static void stm32_writereg(FAR struct stm32_dev_s *priv, uint8_t regaddr, + uint16_t regval); +static uint16_t stm32_readreg(FAR struct stm32_dev_s *priv, uint8_t regaddr); +static void stm32_gramselect(FAR struct stm32_dev_s *priv); +static void stm32_writegram(FAR struct stm32_dev_s *priv, uint16_t rgbval); +static inline uint16_t stm32_readgram(FAR struct stm32_dev_s *priv); +static void stm32_readnosetup(FAR struct stm32_dev_s *priv, FAR uint16_t *accum); +static uint16_t stm32_readnoshift(FAR struct stm32_dev_s *priv, FAR uint16_t *accum); +static void stm32_setcursor(FAR struct stm32_dev_s *priv, uint16_t col, uint16_t row); + +/* LCD Data Transfer Methods */ + +static int stm32_putrun(fb_coord_t row, fb_coord_t col, FAR const uint8_t *buffer, + size_t npixels); +static int stm32_getrun(fb_coord_t row, fb_coord_t col, FAR uint8_t *buffer, + size_t npixels); + +/* LCD Configuration */ + +static int stm32_getvideoinfo(FAR struct lcd_dev_s *dev, + FAR struct fb_videoinfo_s *vinfo); +static int stm32_getplaneinfo(FAR struct lcd_dev_s *dev, unsigned int planeno, + FAR struct lcd_planeinfo_s *pinfo); + +/* LCD RGB Mapping */ + +#ifdef CONFIG_FB_CMAP +# error "RGB color mapping not supported by this driver" +#endif + +/* Cursor Controls */ + +#ifdef CONFIG_FB_HWCURSOR +# error "Cursor control not supported by this driver" +#endif + +/* LCD Specific Controls */ + +static int stm32_getpower(struct lcd_dev_s *dev); +static int stm32_setpower(struct lcd_dev_s *dev, int power); +static int stm32_getcontrast(struct lcd_dev_s *dev); +static int stm32_setcontrast(struct lcd_dev_s *dev, unsigned int contrast); + +/* Initialization */ + +static void stm32_lcdinput(FAR struct stm32_dev_s *priv); +static void stm32_lcdoutput(FAR struct stm32_dev_s *priv); + +#if !defined(CONFIG_STM32_ILI9300_DISABLE) || !defined(CONFIG_STM32_ILI9320_DISABLE) || !defined(CONFIG_STM32_ILI9321_DISABLE) +static void stm32_lcd9300init(FAR struct stm32_dev_s *priv, enum lcd_type_e lcdtype); +#endif +#if !defined(CONFIG_STM32_ILI9325_DISABLE) || !defined(CONFIG_STM32_ILI9328_DISABLE) +static void stm32_lcd9325init(FAR struct stm32_dev_s *priv, enum lcd_type_e lcdtype); +#endif +#ifndef CONFIG_STM32_ILI9919_DISABLE +static inline void stm32_lcd9919init(FAR struct stm32_dev_s *priv); +#endif +#ifndef CONFIG_STM32_ILI1505_DISABLE +static inline void stm32_lcd1505init(FAR struct stm32_dev_s *priv); +#endif +static inline void stm32_lcdinitialize(FAR struct stm32_dev_s *priv); + +/************************************************************************************ + * Private Data + ************************************************************************************/ +/* LCD GPIO configurations */ + +#ifndef CONFIG_LCD_FASTCONFIG +static const uint32_t g_lcdout[16] = +{ + GPIO_LCD_D0OUT, GPIO_LCD_D1OUT, GPIO_LCD_D2OUT, GPIO_LCD_D3OUT, + GPIO_LCD_D4OUT, GPIO_LCD_D5OUT, GPIO_LCD_D6OUT, GPIO_LCD_D7OUT, + GPIO_LCD_D8OUT, GPIO_LCD_D9OUT, GPIO_LCD_D10OUT, GPIO_LCD_D11OUT, + GPIO_LCD_D12OUT, GPIO_LCD_D13OUT, GPIO_LCD_D14OUT, GPIO_LCD_D15OUT +}; + +static const uint32_t g_lcdin[16] = +{ + GPIO_LCD_D0IN, GPIO_LCD_D1IN, GPIO_LCD_D2IN, GPIO_LCD_D3IN, + GPIO_LCD_D4IN, GPIO_LCD_D5IN, GPIO_LCD_D6IN, GPIO_LCD_D7IN, + GPIO_LCD_D8IN, GPIO_LCD_D9IN, GPIO_LCD_D10IN, GPIO_LCD_D11IN, + GPIO_LCD_D12IN, GPIO_LCD_D13IN, GPIO_LCD_D14IN, GPIO_LCD_D15IN +}; +#endif + +static const uint32_t g_lcdctrl[] = +{ + GPIO_LCD_RS, GPIO_LCD_CS, GPIO_LCD_RD, GPIO_LCD_WR, + GPIO_LCD_LE, +}; +#define NLCD_CONFIG (sizeof(g_lcdctrl)/sizeof(uint32_t)) + +/* This is working memory allocated by the LCD driver for each LCD device + * and for each color plane. This memory will hold one raster line of data. + * The size of the allocated run buffer must therefore be at least + * (bpp * xres / 8). Actual alignment of the buffer must conform to the + * bitwidth of the underlying pixel type. + * + * If there are multiple planes, they may share the same working buffer + * because different planes will not be operate on concurrently. However, + * if there are multiple LCD devices, they must each have unique run buffers. + */ + +static uint16_t g_runbuffer[STM32_XRES]; + +/* This structure describes the overall LCD video controller */ + +static const struct fb_videoinfo_s g_videoinfo = +{ + .fmt = STM32_COLORFMT, /* Color format: RGB16-565: RRRR RGGG GGGB BBBB */ + .xres = STM32_XRES, /* Horizontal resolution in pixel columns */ + .yres = STM32_YRES, /* Vertical resolution in pixel rows */ + .nplanes = 1, /* Number of color planes supported */ +}; + +/* This is the standard, NuttX Plane information object */ + +static const struct lcd_planeinfo_s g_planeinfo = +{ + .putrun = stm32_putrun, /* Put a run into LCD memory */ + .getrun = stm32_getrun, /* Get a run from LCD memory */ + .buffer = (uint8_t*)g_runbuffer, /* Run scratch buffer */ + .bpp = STM32_BPP, /* Bits-per-pixel */ +}; + +/* This is the standard, NuttX LCD driver object */ + +static struct stm32_dev_s g_lcddev = +{ + .dev = + { + /* LCD Configuration */ + + .getvideoinfo = stm32_getvideoinfo, + .getplaneinfo = stm32_getplaneinfo, + + /* LCD RGB Mapping -- Not supported */ + /* Cursor Controls -- Not supported */ + + /* LCD Specific Controls */ + + .getpower = stm32_getpower, + .setpower = stm32_setpower, + .getcontrast = stm32_getcontrast, + .setcontrast = stm32_setcontrast, + }, +}; + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: stm32_writereg + * + * Description: + * Write to an LCD register + * + ************************************************************************************/ + +static void stm32_writereg(FAR struct stm32_dev_s *priv, uint8_t regaddr, uint16_t regval) +{ + /* Make sure that we are configured for output */ + + stm32_lcdoutput(priv); + + /* Write the 8-bit register index */ + + putreg32(1, LCD_CS_CLEAR); + putreg32(1, LCD_RS_CLEAR); + putreg32(1, LCD_WR_CLEAR); + putreg32((uint32_t)regaddr, LCD_ODR); + putreg32(1, LCD_WR_SET); + + /* Then write the 16-bit register value */ + + putreg32(1, LCD_RS_CLEAR); + putreg32(1, LCD_WR_CLEAR); + putreg32((uint32_t)regval, LCD_ODR); + putreg32(1, LCD_WR_SET); + putreg32(1, LCD_CS_SET); +} + +/************************************************************************************ + * Name: stm32_readreg + * + * Description: + * Read from an LCD register + * + ************************************************************************************/ + +static uint16_t stm32_readreg(FAR struct stm32_dev_s *priv, uint8_t regaddr) +{ + uint16_t regval; + + /* Make sure that we are configured for output */ + + stm32_lcdoutput(priv); + + /* Write the 8-bit register index */ + + putreg32(1, LCD_CS_CLEAR); + putreg32(1, LCD_RS_CLEAR); + putreg32(1, LCD_WR_CLEAR); + putreg32((uint32_t)regaddr, LCD_ODR); + putreg32(1, LCD_WR_SET); + + /* Make sure that we are configure for input */ + + stm32_lcdinput(priv); + + /* Read the 16-bit register value */ + + putreg32(1, LCD_RS_CLEAR); + putreg32(1, LCD_RD_CLEAR); + putreg32(1, LCD_RD_SET); + regval = (uint16_t)getreg32(LCD_IDR); + putreg32(1, LCD_CS_SET); + + return regval; +} + +/************************************************************************************ + * Name: stm32_gramselect + * + * Description: + * Setup to read or write multiple pixels to the GRAM memory + * + ************************************************************************************/ + +static void stm32_gramselect(FAR struct stm32_dev_s *priv) +{ + /* Make sure that we are configured for output */ + + stm32_lcdoutput(priv); + + /* Write the command */ + + putreg32(1, LCD_CS_CLEAR); + putreg32(1, LCD_RS_CLEAR); + putreg32(1, LCD_WR_CLEAR); + putreg32((uint32_t)LCD_REG_34, LCD_ODR); + putreg32(1, LCD_WR_SET); + putreg32(1, LCD_CS_SET); +} + +/************************************************************************************ + * Name: stm32_writegram + * + * Description: + * Write one pixel to the GRAM memory + * + ************************************************************************************/ + +static inline void stm32_writegram(FAR struct stm32_dev_s *priv, uint16_t rgbval) +{ + /* Make sure that we are configured for output */ + + stm32_lcdoutput(priv); + + /* Write the value (GRAM register already selected) */ + + putreg32(1, LCD_CS_CLEAR); + putreg32(1, LCD_RS_CLEAR); + putreg32(1, LCD_WR_CLEAR); + putreg32((uint32_t)rgbval, LCD_ODR); + putreg32(1, LCD_WR_SET); + putreg32(1, LCD_CS_SET); +} + +/************************************************************************************ + * Name: stm32_readgram + * + * Description: + * Read one 16-bit pixel to the GRAM memory + * + ************************************************************************************/ + +static inline uint16_t stm32_readgram(FAR struct stm32_dev_s *priv) +{ + uint16_t regval; + + /* Make sure that we are configure for input */ + + stm32_lcdinput(priv); + + /* Read the 16-bit value */ + + putreg32(1, LCD_RS_CLEAR); + putreg32(1, LCD_RD_CLEAR); + putreg32(1, LCD_RD_SET); + regval = (uint16_t)getreg32(LCD_IDR); + putreg32(1, LCD_CS_SET); + + return regval; +} + +/************************************************************************************ + * Name: stm32_readnosetup + * + * Description: + * Prime the operation by reading one pixel from the GRAM memory if necessary for + * this LCD type. When reading 16-bit gram data, there may be some shifts in the + * returned data: + * + * - ILI932x: Discard first dummy read; no shift in the return data + * + ************************************************************************************/ + +static void stm32_readnosetup(FAR struct stm32_dev_s *priv, FAR uint16_t *accum) +{ + /* Read-ahead one pixel */ + + *accum = stm32_readgram(priv); +} + +/************************************************************************************ + * Name: stm32_readnoshift + * + * Description: + * Read one correctly aligned pixel from the GRAM memory. Possibly shifting the + * data and possibly swapping red and green components. + * + * - ILI932x: Unknown -- assuming colors are in the color order + * + ************************************************************************************/ + +static uint16_t stm32_readnoshift(FAR struct stm32_dev_s *priv, FAR uint16_t *accum) +{ + /* Read the value (GRAM register already selected) */ + + return stm32_readgram(priv); +} + +/************************************************************************************ + * Name: stm32_setcursor + * + * Description: + * Set the cursor position. In landscape mode, the "column" is actually the physical + * Y position and the "row" is the physical X position. + * + ************************************************************************************/ + +static void stm32_setcursor(FAR struct stm32_dev_s *priv, uint16_t col, uint16_t row) +{ + stm32_writereg(priv, LCD_REG_32, row); /* GRAM horizontal address */ + stm32_writereg(priv, LCD_REG_33, col); /* GRAM vertical address */ +} + +/************************************************************************************ + * Name: stm32_dumprun + * + * Description: + * Dump the contexts of the run buffer: + * + * run - The buffer in containing the run read to be dumped + * npixels - The number of pixels to dump + * + ************************************************************************************/ + +#if 0 /* Sometimes useful */ +static void stm32_dumprun(FAR const char *msg, FAR uint16_t *run, size_t npixels) +{ + int i, j; + + lib_rawprintf("\n%s:\n", msg); + for (i = 0; i < npixels; i += 16) + { + up_putc(' '); + lib_rawprintf(" "); + for (j = 0; j < 16; j++) + { + lib_rawprintf(" %04x", *run++); + } + up_putc('\n'); + } +} +#endif + +/************************************************************************************ + * Name: stm32_putrun + * + * Description: + * This method can be used to write a partial raster line to the LCD: + * + * row - Starting row to write to (range: 0 <= row < yres) + * col - Starting column to write to (range: 0 <= col <= xres-npixels) + * buffer - The buffer containing the run to be written to the LCD + * npixels - The number of pixels to write to the LCD + * (range: 0 < npixels <= xres-col) + * + ************************************************************************************/ + +static int stm32_putrun(fb_coord_t row, fb_coord_t col, FAR const uint8_t *buffer, + size_t npixels) +{ + FAR struct stm32_dev_s *priv = &g_lcddev; + FAR const uint16_t *src = (FAR const uint16_t*)buffer; + int i; + + /* Buffer must be provided and aligned to a 16-bit address boundary */ + + lcdvdbg("row: %d col: %d npixels: %d\n", row, col, npixels); + DEBUGASSERT(buffer && ((uintptr_t)buffer & 1) == 0); + + /* Write the run to GRAM */ + +#ifdef CONFIG_LCD_LANDSCAPE + /* Convert coordinates */ + + /* Write the GRAM data, manually incrementing X */ + + for (i = 0; i < npixels; i++) + { + /* Write the next pixel to this position */ + + stm32_setcursor(priv, col, row); + stm32_gramselect(priv); + stm32_writegram(priv, *src++); + + /* Increment to next column */ + + col++; + } +#elif defined(CONFIG_LCD_RLANDSCAPE) + /* Convert coordinates */ + + col = (STM32_XRES-1) - col; + row = (STM32_YRES-1) - row; + + /* Set the cursor position */ + + stm32_setcursor(priv, col, row); + + /* Then write the GRAM data, auto-decrementing X */ + + stm32_gramselect(priv); + for (i = 0; i < npixels; i++) + { + /* Write the next pixel to this position (auto-decrements to the next column) */ + + stm32_writegram(priv, *src++); + } +#elif defined(CONFIG_LCD_PORTRAIT) + /* Convert coordinates */ + + col = (STM32_XRES-1) - col; + + /* Then write the GRAM data, manually incrementing Y (which is col) */ + + for (i = 0; i < npixels; i++) + { + /* Write the next pixel to this position */ + + stm32_setcursor(priv, row, col); + stm32_gramselect(priv); + stm32_writegram(priv, *src++); + + /* Increment to next column */ + + col--; + } +#else /* CONFIG_LCD_RPORTRAIT */ + /* Convert coordinates */ + + row = (STM32_YRES-1) - row; + + /* Then write the GRAM data, manually incrementing Y (which is col) */ + + for (i = 0; i < npixels; i++) + { + /* Write the next pixel to this position */ + + stm32_setcursor(priv, row, col); + stm32_gramselect(priv); + stm32_writegram(priv, *src++); + + /* Decrement to next column */ + + col++; + } +#endif + return OK; +} + +/************************************************************************************ + * Name: stm32_getrun + * + * Description: + * This method can be used to read a partial raster line from the LCD: + * + * row - Starting row to read from (range: 0 <= row < yres) + * col - Starting column to read read (range: 0 <= col <= xres-npixels) + * buffer - The buffer in which to return the run read from the LCD + * npixels - The number of pixels to read from the LCD + * (range: 0 < npixels <= xres-col) + * + ************************************************************************************/ + +static int stm32_getrun(fb_coord_t row, fb_coord_t col, FAR uint8_t *buffer, + size_t npixels) +{ + FAR struct stm32_dev_s *priv = &g_lcddev; + FAR uint16_t *dest = (FAR uint16_t*)buffer; + void (*readsetup)(FAR struct stm32_dev_s *priv, FAR uint16_t *accum); + uint16_t (*readgram)(FAR struct stm32_dev_s *priv, FAR uint16_t *accum); + uint16_t accum; + int i; + + /* Buffer must be provided and aligned to a 16-bit address boundary */ + + lcdvdbg("row: %d col: %d npixels: %d\n", row, col, npixels); + DEBUGASSERT(buffer && ((uintptr_t)buffer & 1) == 0); + + /* Configure according to the LCD type. Kind of silly with only one LCD type */ + + switch (priv->type) + { + case LCD_TYPE_ILI1505: + case LCD_TYPE_ILI9300: + case LCD_TYPE_ILI9320: + case LCD_TYPE_ILI9321: + case LCD_TYPE_ILI9325: + case LCD_TYPE_ILI9328: + case LCD_TYPE_ILI9331: + case LCD_TYPE_ILI9919: + readsetup = stm32_readnosetup; + readgram = stm32_readnoshift; + break; + + case LCD_TYPE_UNKNOWN: + default: /* Shouldn't happen */ + return -ENOSYS; + } + + /* Read the run from GRAM */ + +#ifdef CONFIG_LCD_LANDSCAPE + /* Convert coordinates */ + + for (i = 0; i < npixels; i++) + { + /* Read the next pixel from this position */ + + stm32_setcursor(priv, row, col); + stm32_gramselect(priv); + stm32_lcdinput(priv); + readsetup(priv, &accum); + *dest++ = readgram(priv, &accum); + + /* Increment to next column */ + + col++; + } +#elif defined(CONFIG_LCD_RLANDSCAPE) + /* Convert coordinates */ + + col = (STM32_XRES-1) - col; + row = (STM32_YRES-1) - row; + + /* Set the cursor position */ + + stm32_setcursor(priv, col, row); + + /* Then read the GRAM data, auto-decrementing Y */ + + stm32_gramselect(priv); + stm32_lcdinput(priv); + + /* Prime the pump for unaligned read data */ + + readsetup(priv, &accum); + + for (i = 0; i < npixels; i++) + { + /* Read the next pixel from this position (autoincrements to the next row) */ + + *dest++ = readgram(priv, &accum); + } +#elif defined(CONFIG_LCD_PORTRAIT) + /* Convert coordinates */ + + col = (STM32_XRES-1) - col; + + /* Then read the GRAM data, manually incrementing Y (which is col) */ + + for (i = 0; i < npixels; i++) + { + /* Read the next pixel from this position */ + + stm32_setcursor(priv, row, col); + stm32_gramselect(priv); + stm32_lcdinput(priv); + readsetup(priv, &accum); + *dest++ = readgram(priv, &accum); + + /* Increment to next column */ + + col--; + } +#else /* CONFIG_LCD_RPORTRAIT */ + /* Convert coordinates */ + + row = (STM32_YRES-1) - row; + + /* Then write the GRAM data, manually incrementing Y (which is col) */ + + for (i = 0; i < npixels; i++) + { + /* Write the next pixel to this position */ + + stm32_setcursor(priv, row, col); + stm32_gramselect(priv); + stm32_lcdinput(priv); + readsetup(priv, &accum); + *dest++ = readgram(priv, &accum); + + /* Decrement to next column */ + + col++; + } +#endif + + return OK; +} + +/************************************************************************************ + * Name: stm32_getvideoinfo + * + * Description: + * Get information about the LCD video controller configuration. + * + ************************************************************************************/ + +static int stm32_getvideoinfo(FAR struct lcd_dev_s *dev, + FAR struct fb_videoinfo_s *vinfo) +{ + DEBUGASSERT(dev && vinfo); + lcdvdbg("fmt: %d xres: %d yres: %d nplanes: %d\n", + g_videoinfo.fmt, g_videoinfo.xres, g_videoinfo.yres, g_videoinfo.nplanes); + memcpy(vinfo, &g_videoinfo, sizeof(struct fb_videoinfo_s)); + return OK; +} + +/************************************************************************************ + * Name: stm32_getplaneinfo + * + * Description: + * Get information about the configuration of each LCD color plane. + * + ************************************************************************************/ + +static int stm32_getplaneinfo(FAR struct lcd_dev_s *dev, unsigned int planeno, + FAR struct lcd_planeinfo_s *pinfo) +{ + DEBUGASSERT(dev && pinfo && planeno == 0); + lcdvdbg("planeno: %d bpp: %d\n", planeno, g_planeinfo.bpp); + memcpy(pinfo, &g_planeinfo, sizeof(struct lcd_planeinfo_s)); + return OK; +} + +/************************************************************************************ + * Name: stm32_getpower + * + * Description: + * Get the LCD panel power status (0: full off - CONFIG_LCD_MAXPOWER: full on). On + * backlit LCDs, this setting may correspond to the backlight setting. + * + ************************************************************************************/ + +static int stm32_getpower(struct lcd_dev_s *dev) +{ + FAR struct stm32_dev_s *priv = (FAR struct stm32_dev_s *)dev; + + lcdvdbg("power: %d\n", 0); + return priv->power; +} + +/************************************************************************************ + * Name: stm32_poweroff + * + * Description: + * Enable/disable LCD panel power (0: full off - CONFIG_LCD_MAXPOWER: full on). On + * backlit LCDs, this setting may correspond to the backlight setting. + * + ************************************************************************************/ + +static int stm32_poweroff(FAR struct stm32_dev_s *priv) +{ + /* Turn the display off */ + + stm32_writereg(priv, LCD_REG_7, 0); + + /* Remember the power off state */ + + priv->power = 0; + return OK; +} + +/************************************************************************************ + * Name: stm32_setpower + * + * Description: + * Enable/disable LCD panel power (0: full off - CONFIG_LCD_MAXPOWER: full on). On + * backlit LCDs, this setting may correspond to the backlight setting. + * + ************************************************************************************/ + +static int stm32_setpower(struct lcd_dev_s *dev, int power) +{ + FAR struct stm32_dev_s *priv = (FAR struct stm32_dev_s *)dev; + + lcdvdbg("power: %d\n", power); + DEBUGASSERT((unsigned)power <= CONFIG_LCD_MAXPOWER); + + /* Set new power level */ + + if (power > 0) + { + /* Then turn the display on */ + +#ifndef CONFIG_STM32_ILI9300_DISABLE + if (priv->type == LCD_TYPE_ILI9300) + { + stm32_writereg(priv, LCD_REG_7, 0x0173); + } + else +#endif +#ifndef CONFIG_STM32_ILI9320_DISABLE + if (priv->type == LCD_TYPE_ILI9300) + { + stm32_writereg(priv, LCD_REG_7, 0x0173); + } + else +#endif +#ifndef CONFIG_STM32_ILI9321_DISABLE + if (priv->type == LCD_TYPE_ILI9300) + { + stm32_writereg(priv, LCD_REG_7, 0x0173); + } + else +#endif +#ifndef CONFIG_STM32_ILI9325_DISABLE + if (priv->type == LCD_TYPE_ILI9325) + { + stm32_writereg(priv, LCD_REG_7, 0x0133); + } + else +#endif +#ifndef CONFIG_STM32_ILI9328_DISABLE + if (priv->type == LCD_TYPE_ILI9328) + { + stm32_writereg(priv, LCD_REG_7, 0x0133); + } + else +#endif +#ifndef CONFIG_STM32_ILI9331_DISABLE + if (priv->type == LCD_TYPE_ILI9331) + { + stm32_writereg(priv, LCD_REG_7, 0x0021); + up_mdelay(50); + stm32_writereg(priv, LCD_REG_7, 0x0061); + up_mdelay(50); + stm32_writereg(priv, LCD_REG_7, 0x0133); /* 262K color and display ON */ + } + else +#endif +#ifndef CONFIG_STM32_ILI9919_DISABLE + if (priv->type == LCD_TYPE_ILI9919) + { + stm32_writereg(priv, LCD_REG_7, 0x0033); + } + else +#endif +#ifndef CONFIG_STM32_ILI1505_DISABLE + if (priv->type == LCD_TYPE_ILI1505) + { + stm32_writereg(priv, LCD_REG_7, 0x0021); + up_mdelay(20); + stm32_writereg(priv, LCD_REG_7, 0x0061); + up_mdelay(20); + stm32_writereg(priv, LCD_REG_7, 0x0173); + } + else +#endif + { + gdbg("Unsupported LCD: %d\n", priv->type); + } + + up_mdelay(50); + priv->power = power; + } + else + { + /* Turn the display off */ + + stm32_poweroff(priv); + } + + return OK; +} + +/************************************************************************************ + * Name: stm32_getcontrast + * + * Description: + * Get the current contrast setting (0-CONFIG_LCD_MAXCONTRAST). + * + ************************************************************************************/ + +static int stm32_getcontrast(struct lcd_dev_s *dev) +{ + lcdvdbg("Not implemented\n"); + return -ENOSYS; +} + +/************************************************************************************ + * Name: stm32_setcontrast + * + * Description: + * Set LCD panel contrast (0-CONFIG_LCD_MAXCONTRAST). + * + ************************************************************************************/ + +static int stm32_setcontrast(struct lcd_dev_s *dev, unsigned int contrast) +{ + lcdvdbg("contrast: %d\n", contrast); + return -ENOSYS; +} + +/************************************************************************************ + * Name: stm32_lcdinput + * + * Description: + * Config data lines for input operations. + * + ************************************************************************************/ + +static void stm32_lcdinput(FAR struct stm32_dev_s *priv) +{ +#ifndef CONFIG_LCD_FASTCONFIG + int i; +#endif + + /* Check if we are already configured for input */ + + if (priv->output) + { + /* Configure GPIO data lines as inputs */ + +#ifdef CONFIG_LCD_FASTCONFIG + putreg32(LCD_INPUT, LCD_CRL); + putreg32(LCD_INPUT, LCD_CRH); +#else + for (i = 0; i < 16; i++) + { + stm32_configgpio(g_lcdin[i]); + } +#endif + /* No longer configured for output */ + + priv->output = false; + } +} + +/************************************************************************************ + * Name: stm32_lcdoutput + * + * Description: + * Config data lines for input operations. + * + ************************************************************************************/ + +static void stm32_lcdoutput(FAR struct stm32_dev_s *priv) +{ +#ifndef CONFIG_LCD_FASTCONFIG + int i; +#endif + + /* Check if we are already configured for output */ + + if (!priv->output) + { + /* Configure GPIO data lines as outputs */ + +#ifdef CONFIG_LCD_FASTCONFIG + putreg32(LCD_OUTPUT, LCD_CRL); + putreg32(LCD_OUTPUT, LCD_CRH); +#else + for (i = 0; i < 16; i++) + { + stm32_configgpio(g_lcdout[i]); + } +#endif + /* Now we are configured for output */ + + priv->output = true; + } +} + +/************************************************************************************ + * Name: stm32_lcd9300init + * + * Description: + * Initialize the ILI9300/9220/9321 LCD. + * + ************************************************************************************/ + +#if !defined(CONFIG_STM32_ILI9300_DISABLE) || !defined(CONFIG_STM32_ILI9320_DISABLE) || !defined(CONFIG_STM32_ILI9321_DISABLE) +static void stm32_lcd9300init(FAR struct stm32_dev_s *priv, enum lcd_type_e lcdtype) +{ + stm32_writereg(priv, LCD_REG_0, 0x0001); /* Start internal OSC */ + stm32_writereg(priv, LCD_REG_1, 0x0100); /* Driver Output Control */ + stm32_writereg(priv, LCD_REG_2, 0x0700); /* LCD Driver Waveform Control */ + stm32_writereg(priv, LCD_REG_3, 0x1018); /* Set GRAM write direction and BGR=1 (0x1030)*/ + + stm32_writereg(priv, LCD_REG_4, 0x0000); /* Scalling Control */ + stm32_writereg(priv, LCD_REG_8, 0x0202); /* Set the back porch and front porch (0x0207) */ + stm32_writereg(priv, LCD_REG_9, 0x0000); /* Set non-display area refresh cycle ISC[3:0] */ + stm32_writereg(priv, LCD_REG_10, 0x0000); /* Frame Cycle Control */ + stm32_writereg(priv, LCD_REG_12, (1<<0)); /* RGB interface setting (0x0000) */ + stm32_writereg(priv, LCD_REG_13, 0x0000); /* Frame Maker Position */ + stm32_writereg(priv, LCD_REG_15, 0x0000); /* RGB interface polarity */ + + up_mdelay(50); + stm32_writereg(priv, LCD_REG_7, 0x0101); /* Display Control */ + up_mdelay(50); + + /* Power On sequence */ + + stm32_writereg(priv, LCD_REG_16, (1<<12)|(0<<8)|(1<<7)|(1<<6)|(0<<4)); /* Power Control 1 (0x16b0) */ + stm32_writereg(priv, LCD_REG_17, 0x0007); /* Power Control 2 (0x0001) */ + stm32_writereg(priv, LCD_REG_18, (1<<8)|(1<<4)|(0<<0)); /* Power Control 3 (0x0138) */ + stm32_writereg(priv, LCD_REG_19, 0x0b00); /* VDV[4:0] for VCOM amplitude */ + stm32_writereg(priv, LCD_REG_41, 0x0000); /* VCM[4:0] for VCOMH */ + + stm32_writereg(priv, LCD_REG_43, (1<<14)|(1<<4)); + + stm32_writereg(priv, LCD_REG_80, 0); /* Set X Start */ + stm32_writereg(priv, LCD_REG_81, 239); /* Set X End */ + stm32_writereg(priv, LCD_REG_82, 0); /* Set Y Start */ + stm32_writereg(priv, LCD_REG_83, 319); /* Set Y End */ + + stm32_writereg(priv, LCD_REG_96, 0x2700); /* Driver Output Control */ + stm32_writereg(priv, LCD_REG_97, 0x0001); /* Driver Output Control */ + stm32_writereg(priv, LCD_REG_106, 0x0000); /* Vertical Srcoll Control */ + + stm32_writereg(priv, LCD_REG_128, 0x0000); /* Display Position? Partial Display 1 */ + stm32_writereg(priv, LCD_REG_129, 0x0000); /* RAM Address Start? Partial Display 1 */ + stm32_writereg(priv, LCD_REG_130, 0x0000); /* RAM Address End-Partial Display 1 */ + stm32_writereg(priv, LCD_REG_131, 0x0000); /* Display Position? Partial Display 2 */ + stm32_writereg(priv, LCD_REG_132, 0x0000); /* RAM Address Start? Partial Display 2 */ + stm32_writereg(priv, LCD_REG_133, 0x0000); /* RAM Address End? Partial Display 2 */ + + stm32_writereg(priv, LCD_REG_144, (0<<7)|(16<<0)); /* Frame Cycle Control (0x0013) */ + stm32_writereg(priv, LCD_REG_146, 0x0000); /* Panel Interface Control 2 */ + stm32_writereg(priv, LCD_REG_147, 0x0001); /* Panel Interface Control 3 */ + stm32_writereg(priv, LCD_REG_149, 0x0110); /* Frame Cycle Control */ + stm32_writereg(priv, LCD_REG_151, (0<<8)); + stm32_writereg(priv, LCD_REG_152, 0x0000); /* Frame Cycle Control */ + up_mdelay(50); + stm32_writereg(priv, LCD_REG_7, 0x0000); /* Display off */ +} +#endif + +/************************************************************************************ + * Name: stm32_lcd9331init + * + * Description: + * Initialize the ILI9331 LCD. + * + ************************************************************************************/ + +#ifndef CONFIG_STM32_ILI9331_DISABLE +static void stm32_lcd9331init(FAR struct stm32_dev_s *priv) +{ + stm32_writereg(priv, LCD_REG_231, 0x1014); + stm32_writereg(priv, LCD_REG_1, 0x0100); /* Set SS and SM bit */ + stm32_writereg(priv, LCD_REG_2, 0x0200); /* Set 1 line inversion */ + stm32_writereg(priv, LCD_REG_3, 0x1030); /* Set GRAM write direction and BGR=1 */ + stm32_writereg(priv, LCD_REG_8, 0x0202); /* Set the back porch and front porch */ + stm32_writereg(priv, LCD_REG_9, 0x0000); /* Set non-display area refresh cycle ISC[3:0] */ + stm32_writereg(priv, LCD_REG_10, 0x0000); /* FMARK function */ + stm32_writereg(priv, LCD_REG_12, 0x0000); /* RGB interface setting */ + stm32_writereg(priv, LCD_REG_13, 0x0000); /* Frame marker Position */ + stm32_writereg(priv, LCD_REG_15, 0x0000); /* RGB interface polarity */ + + /* Power On sequence */ + + stm32_writereg(priv, LCD_REG_16, 0x0000); /* SAP, BT[3:0], AP, DSTB, SLP, STB */ + stm32_writereg(priv, LCD_REG_17, 0x0007); /* DC1[2:0], DC0[2:0], VC[2:0] */ + stm32_writereg(priv, LCD_REG_18, 0x0000); /* VREG1OUT voltage */ + stm32_writereg(priv, LCD_REG_19, 0x0000); /* VDV[4:0] for VCOM amplitude */ + up_mdelay(200); /* Dis-charge capacitor power voltage */ + stm32_writereg(priv, LCD_REG_16, 0x1690); /* SAP, BT[3:0], AP, DSTB, SLP, STB */ + stm32_writereg(priv, LCD_REG_17, 0x0227); /* DC1[2:0], DC0[2:0], VC[2:0] */ + up_mdelay(50); + stm32_writereg(priv, LCD_REG_18, 0x000c); /* Internal reference voltage= Vci; */ + up_mdelay(50); + stm32_writereg(priv, LCD_REG_19, 0x0800); /* Set VDV[4:0] for VCOM amplitude */ + stm32_writereg(priv, LCD_REG_41, 0x0011); /* Set VCM[5:0] for VCOMH */ + stm32_writereg(priv, LCD_REG_43, 0x000b); /* Set Frame Rate */ + up_mdelay(50); + stm32_writereg(priv, LCD_REG_32, 0x0000); /* GRAM horizontal Address */ + stm32_writereg(priv, LCD_REG_33, 0x0000); /* GRAM Vertical Address */ + + /* Adjust the Gamma Curve */ + + stm32_writereg(priv, LCD_REG_48, 0x0000); + stm32_writereg(priv, LCD_REG_49, 0x0106); + stm32_writereg(priv, LCD_REG_50, 0x0000); + stm32_writereg(priv, LCD_REG_53, 0x0204); + stm32_writereg(priv, LCD_REG_54, 0x160a); + stm32_writereg(priv, LCD_REG_55, 0x0707); + stm32_writereg(priv, LCD_REG_56, 0x0106); + stm32_writereg(priv, LCD_REG_57, 0x0707); + stm32_writereg(priv, LCD_REG_60, 0x0402); + stm32_writereg(priv, LCD_REG_61, 0x0c0f); + + /* Set GRAM area */ + + stm32_writereg(priv, LCD_REG_80, 0x0000); /* Horizontal GRAM Start Address */ + stm32_writereg(priv, LCD_REG_81, 0x00ef); /* Horizontal GRAM End Address */ + stm32_writereg(priv, LCD_REG_82, 0x0000); /* Vertical GRAM Start Address */ + stm32_writereg(priv, LCD_REG_83, 0x013f); /* Vertical GRAM Start Address */ + stm32_writereg(priv, LCD_REG_96, 0x2700); /* Gate Scan Line */ + stm32_writereg(priv, LCD_REG_97, 0x0001); /* NDL,VLE, REV */ + stm32_writereg(priv, LCD_REG_106, 0x0000); /* set scrolling line */ + + /* Partial Display Control */ + + stm32_writereg(priv, LCD_REG_128, 0x0000); + stm32_writereg(priv, LCD_REG_129, 0x0000); + stm32_writereg(priv, LCD_REG_130, 0x0000); + stm32_writereg(priv, LCD_REG_131, 0x0000); + stm32_writereg(priv, LCD_REG_132, 0x0000); + stm32_writereg(priv, LCD_REG_133, 0x0000); + + /* Panel Control */ + + stm32_writereg(priv, LCD_REG_144, 0x0010); + stm32_writereg(priv, LCD_REG_146, 0x0600); + stm32_writereg(priv, LCD_REG_7, 0x0000); /* Display off */ +} +#endif + +/************************************************************************************ + * Name: stm32_lcd9325init + * + * Description: + * Initialize the ILI9325/9228 LCD. + * + ************************************************************************************/ + +#if !defined(CONFIG_STM32_ILI9325_DISABLE) || !defined(CONFIG_STM32_ILI9328_DISABLE) +static void stm32_lcd9325init(FAR struct stm32_dev_s *priv, enum lcd_type_e lcdtype) +{ + stm32_writereg(priv, LCD_REG_227, 0x3008); + stm32_writereg(priv, LCD_REG_231, 0x0012); + stm32_writereg(priv, LCD_REG_239, 0x1231); /* Set the internal vcore voltage */ +/*stm32_writereg(priv, LCD_REG_231, 0x0010); */ + stm32_writereg(priv, LCD_REG_0, 0x0001); /* Start internal osc */ + stm32_writereg(priv, LCD_REG_1, 0x0100); /* Set SS and SM bit */ + stm32_writereg(priv, LCD_REG_2, 0x0700); /* Power on sequence */ + stm32_writereg(priv, LCD_REG_3, (1<<12)|(1<<5)|(1<<4) ); /* 65K */ + stm32_writereg(priv, LCD_REG_4, 0x0000); /* Resize register */ + stm32_writereg(priv, LCD_REG_8, 0x0207); /* Set the back porch and front porch */ + stm32_writereg(priv, LCD_REG_9, 0x0000); /* Set non-display area refresh cycle ISC[3:0] */ + stm32_writereg(priv, LCD_REG_10, 0x0000); /* FMARK function */ + stm32_writereg(priv, LCD_REG_12, 0x0001); /* RGB interface setting */ + stm32_writereg(priv, LCD_REG_13, 0x0000); /* Frame marker Position (0x0f3c) */ + stm32_writereg(priv, LCD_REG_15, 0x0000); /* RGB interface polarity */ + + /* Power On sequence */ + + stm32_writereg(priv, LCD_REG_16, 0x0000); /* SAP, BT[3:0], AP, DSTB, SLP, STB */ + stm32_writereg(priv, LCD_REG_17, 0x0007); /* DC1[2:0], DC0[2:0], VC[2:0] */ + stm32_writereg(priv, LCD_REG_18, 0x0000); /* VREG1OUT voltage */ + stm32_writereg(priv, LCD_REG_19, 0x0000); /* VDV[4:0] for VCOM amplitude */ + up_mdelay(100); + stm32_writereg(priv, LCD_REG_16, 0x1590); /* SAP, BT[3:0], AP, DSTB, SLP, STB */ + stm32_writereg(priv, LCD_REG_17, 0x0227); /* DC1[2:0], DC0[2:0], VC[2:0] */ + up_mdelay(100); + stm32_writereg(priv, LCD_REG_18, 0x009c); /* VREG1OUT voltage */ + up_mdelay(100); + stm32_writereg(priv, LCD_REG_19, 0x1900); /* VDV[4:0] for VCOM amplitude */ + stm32_writereg(priv, LCD_REG_41, 0x0023); /* VCM[4:0] for VCOMH */ + stm32_writereg(priv, LCD_REG_43, 0x000e); + up_mdelay(100); + stm32_writereg(priv, LCD_REG_32, 0x0000); /* GRAM horizontal Address */ + stm32_writereg(priv, LCD_REG_33, 0x0000); /* GRAM Vertical Address */ + up_mdelay(100); + + /* Adjust the Gamma Curve */ + + stm32_writereg(priv, LCD_REG_48, 0x0007); + stm32_writereg(priv, LCD_REG_49, 0x0707); + stm32_writereg(priv, LCD_REG_50, 0x0006); + stm32_writereg(priv, LCD_REG_53, 0x0704); + stm32_writereg(priv, LCD_REG_54, 0x1f04); + stm32_writereg(priv, LCD_REG_55, 0x0004); + stm32_writereg(priv, LCD_REG_56, 0x0000); + stm32_writereg(priv, LCD_REG_57, 0x0706); + stm32_writereg(priv, LCD_REG_60, 0x0701); + stm32_writereg(priv, LCD_REG_61, 0x000f); + up_mdelay(100); + + /* Set GRAM area */ + + stm32_writereg(priv, LCD_REG_80, 0x0000); /* Horizontal GRAM Start Address */ + stm32_writereg(priv, LCD_REG_81, 0x00ef); /* Horizontal GRAM End Address */ + stm32_writereg(priv, LCD_REG_82, 0x0000); /* Vertical GRAM Start Address */ + stm32_writereg(priv, LCD_REG_83, 0x013f); /* Vertical GRAM End Address */ + stm32_writereg(priv, LCD_REG_96, 0xa700); /* Gate Scan Line */ + stm32_writereg(priv, LCD_REG_97, 0x0001); /* NDL, VLE, REV */ + stm32_writereg(priv, LCD_REG_106, 0x0000); /* Set scrolling line */ + + /* Partial Display Control */ + + stm32_writereg(priv, LCD_REG_128, 0x0000); + stm32_writereg(priv, LCD_REG_129, 0x0000); + stm32_writereg(priv, LCD_REG_130, 0x0000); + stm32_writereg(priv, LCD_REG_131, 0x0000); + stm32_writereg(priv, LCD_REG_132, 0x0000); + stm32_writereg(priv, LCD_REG_133, 0x0000); + + /* Panel Control */ + + stm32_writereg(priv, LCD_REG_144, 0x0010); + stm32_writereg(priv, LCD_REG_146, 0x0600); + + if (lcdtype == LCD_TYPE_ILI9328) + { + stm32_writereg(priv, LCD_REG_147, 0x0003); + stm32_writereg(priv, LCD_REG_149, 0x0110); + stm32_writereg(priv, LCD_REG_151, 0x0000); + stm32_writereg(priv, LCD_REG_152, 0x0000); + } + + stm32_writereg(priv, LCD_REG_7, 0x0000); /* Display off */ + stm32_writereg(priv, LCD_REG_32, 0x0000); /* GRAM horizontal Address */ + stm32_writereg(priv, LCD_REG_33, 0x0000); /* GRAM Vertical Address */ +} +#endif + +/************************************************************************************ + * Name: stm32_lcd9919init + * + * Description: + * Initialize the ILI9919 LCD. + * + ************************************************************************************/ + +#ifndef CONFIG_STM32_ILI9919_DISABLE +static inline void stm32_lcd9919init(FAR struct stm32_dev_s *priv) +{ + /* Power on reset, display off */ + + stm32_writereg(priv, LCD_REG_40, 0x0006); + stm32_writereg(priv, LCD_REG_0, 0x0001); /* Start internal OSC */ + stm32_writereg(priv, LCD_REG_16, 0x0000); /* SAP, BT[3:0], AP, DSTB, SLP, STB */ + stm32_writereg(priv, LCD_REG_1, 0x72ef); + stm32_writereg(priv, LCD_REG_2, 0x0600); /* Set 1 line inversion */ + stm32_writereg(priv, LCD_REG_3, 0x6a38); + stm32_writereg(priv, LCD_REG_17, 0x6874); /* DC1[2:0], DC0[2:0], VC[2:0] (0x0070) */ + + /* RAM write data mask */ + + stm32_writereg(priv, LCD_REG_15, 0x0000); /* RGB interface polarity */ + + stm32_writereg(priv, LCD_REG_11, 0x5308); + stm32_writereg(priv, LCD_REG_12, 0x0003); /* RGB interface setting */ + stm32_writereg(priv, LCD_REG_13, 0x000a); /* Frame marker Position */ + stm32_writereg(priv, LCD_REG_14, 0x2e00); /* 0x0030 */ + stm32_writereg(priv, LCD_REG_30, 0x00be); + stm32_writereg(priv, LCD_REG_37, 0x8000); + stm32_writereg(priv, LCD_REG_38, 0x7800); + stm32_writereg(priv, LCD_REG_39, 0x0078); + stm32_writereg(priv, LCD_REG_78, 0x0000); + stm32_writereg(priv, LCD_REG_79, 0x0000); + stm32_writereg(priv, LCD_REG_18, 0x08d9); /* VREG1OUT voltage */ + + /* Adjust the Gamma Curve */ + + stm32_writereg(priv, LCD_REG_48, 0x0000); /* 0x0007 */ + stm32_writereg(priv, LCD_REG_49, 0x0104); /* 0x0203 */ + stm32_writereg(priv, LCD_REG_50, 0x0100); /* 0x0001 */ + stm32_writereg(priv, LCD_REG_51, 0x0305); /* 0x0007 */ + stm32_writereg(priv, LCD_REG_52, 0x0505); /* 0x0007 */ + stm32_writereg(priv, LCD_REG_53, 0x0305); /* 0x0407 */ + stm32_writereg(priv, LCD_REG_54, 0x0707); /* 0x0407 */ + stm32_writereg(priv, LCD_REG_55, 0x0300); /* 0x0607 */ + stm32_writereg(priv, LCD_REG_58, 0x1200); /* 0x0106 */ + stm32_writereg(priv, LCD_REG_59, 0x0800); + + stm32_writereg(priv, LCD_REG_7, 0x0000); /* Display off */ +} +#endif + +/************************************************************************************ + * Name: stm32_lcd1505init + * + * Description: + * Initialize the ILI1505 LCD. + * + ************************************************************************************/ + +#ifndef CONFIG_STM32_ILI1505_DISABLE +static inline void stm32_lcd1505init(FAR struct stm32_dev_s *priv) +{ + stm32_writereg(priv, LCD_REG_7, 0x0000); + up_mdelay(5); + stm32_writereg(priv, LCD_REG_18, 0x011c); + stm32_writereg(priv, LCD_REG_164, 0x0001); /* NVM */ + stm32_writereg(priv, LCD_REG_8, 0x000f); + stm32_writereg(priv, LCD_REG_10, 0x0008); + stm32_writereg(priv, LCD_REG_13, 0x0008); + + /* Adjust the Gamma Curve */ + + stm32_writereg(priv, LCD_REG_48, 0x0707); + stm32_writereg(priv, LCD_REG_49, 0x0007); /* 0x0707 */ + stm32_writereg(priv, LCD_REG_50, 0x0603); + stm32_writereg(priv, LCD_REG_51, 0x0700); + stm32_writereg(priv, LCD_REG_52, 0x0202); + stm32_writereg(priv, LCD_REG_53, 0x0002); /* 0x0606 */ + stm32_writereg(priv, LCD_REG_54, 0x1f0f); + stm32_writereg(priv, LCD_REG_55, 0x0707); /* 0x0f0f, 0x0105 */ + stm32_writereg(priv, LCD_REG_56, 0x0000); + stm32_writereg(priv, LCD_REG_57, 0x0000); + stm32_writereg(priv, LCD_REG_58, 0x0707); + stm32_writereg(priv, LCD_REG_59, 0x0000); /* 0x0303 */ + stm32_writereg(priv, LCD_REG_60, 0x0007); /* 0x0707 */ + stm32_writereg(priv, LCD_REG_61, 0x0000); /* 0x1313, 0x1f08 */ + up_mdelay(5); + + stm32_writereg(priv, LCD_REG_7, 0x0001); + stm32_writereg(priv, LCD_REG_23, 0x0001); /* Power supply startup enable */ + up_mdelay(5); + + /* Power Control */ + + stm32_writereg(priv, LCD_REG_16, 0x17a0); /* SAP, BT[3:0], AP, DSTB, SLP, STB */ + stm32_writereg(priv, LCD_REG_17, 0x0217); /* Reference voltage VC[2:0] Vciout = 1.00*Vcivl */ + stm32_writereg(priv, LCD_REG_18, 0x011e); /* Vreg1out = Vcilvl*1.80 (0x011c) */ + stm32_writereg(priv, LCD_REG_19, 0x0f00); /* VDV[4:0]-->VCOM Amplitude VcomL = VcomH - Vcom Ampl */ + stm32_writereg(priv, LCD_REG_42, 0x0000); + stm32_writereg(priv, LCD_REG_41, 0x000a); /* Vcomh = VCM1[4:0]*Vreg1out gate source voltage (0x001f) */ + stm32_writereg(priv, LCD_REG_18, 0x013e); /* Power supply on (0x013c) */ + + /* Coordinates Control */ + + stm32_writereg(priv, LCD_REG_80, 0x0000); /* Horizontal GRAM Start Address */ + stm32_writereg(priv, LCD_REG_81, 0x00ef); /* Horizontal GRAM End Address */ + stm32_writereg(priv, LCD_REG_82, 0x0000); /* Vertical GRAM Start Address */ + stm32_writereg(priv, LCD_REG_83, 0x013f); /* Vertical GRAM End Address */ + + /* Panel Image Control */ + + stm32_writereg(priv, LCD_REG_96, 0x2700); /* Gate Scan Line */ + stm32_writereg(priv, LCD_REG_97, 0x0001); /* NDL, VLE, REV */ + stm32_writereg(priv, LCD_REG_106, 0x0000); /* Set scrolling line */ + + /* Partial Image Control */ + + stm32_writereg(priv, LCD_REG_128, 0x0000); + stm32_writereg(priv, LCD_REG_129, 0x0000); + stm32_writereg(priv, LCD_REG_130, 0x0000); + stm32_writereg(priv, LCD_REG_131, 0x0000); + stm32_writereg(priv, LCD_REG_132, 0x0000); + stm32_writereg(priv, LCD_REG_133, 0x0000); + + /* Panel Interface Control */ + + stm32_writereg(priv, LCD_REG_144, 0x0013); + stm32_writereg(priv, LCD_REG_146, 0x0300); + stm32_writereg(priv, LCD_REG_147, 0x0005); + stm32_writereg(priv, LCD_REG_149, 0x0000); + stm32_writereg(priv, LCD_REG_151, 0x0000); + stm32_writereg(priv, LCD_REG_152, 0x0000); + + stm32_writereg(priv, LCD_REG_1, 0x0100); /* Set SS and SM bit */ + stm32_writereg(priv, LCD_REG_2, 0x0700); /* Set 1 line inversion */ + stm32_writereg(priv, LCD_REG_3, 0x1030); /* Set GRAM write direction and BGR=1 */ + stm32_writereg(priv, LCD_REG_4, 0x0000); /* Resize register */ + stm32_writereg(priv, LCD_REG_12, 0x0000); /* RGB interface setting */ + stm32_writereg(priv, LCD_REG_15, 0x0000); /* RGB interface polarity */ + stm32_writereg(priv, LCD_REG_32, 0x0000); /* GRAM horizontal Address */ + stm32_writereg(priv, LCD_REG_33, 0x0000); /* GRAM Vertical Address */ + stm32_writereg(priv, LCD_REG_7, 0x0000); /* Display off */ +} +#endif + +/************************************************************************************ + * Name: stm32_lcdinitialize + * + * Description: + * Set LCD panel contrast (0-CONFIG_LCD_MAXCONTRAST). + * + ************************************************************************************/ + +static inline void stm32_lcdinitialize(FAR struct stm32_dev_s *priv) +{ + uint16_t id; + + /* Check LCD ID */ + + stm32_writereg(priv, LCD_REG_0, 0x0001); /* Start internal oscillator */ + up_mdelay(50); + + id = stm32_readreg(priv, LCD_REG_0); /* Read the ID register */ + lcddbg("LCD ID: %04x\n", id); + + stm32_lcdoutput(priv); + up_mdelay(10); + + /* Intialize the LCD hardware */ + +#ifndef CONFIG_STM32_ILI9300_DISABLE + if (id == ILI9300_ID) + { + priv->type = LCD_TYPE_ILI9300; + stm32_lcd9300init(priv, LCD_TYPE_ILI9325); + } + else +#endif +#ifndef CONFIG_STM32_ILI9320_DISABLE + if (id == ILI9320_ID) + { + priv->type = LCD_TYPE_ILI9320; + stm32_lcd9300init(priv, LCD_TYPE_ILI9320); + } + else +#endif +#ifndef CONFIG_STM32_ILI9321_DISABLE + if (id == ILI9321_ID) + { + priv->type = LCD_TYPE_ILI9321; + stm32_lcd9300init(priv, LCD_TYPE_ILI9321); + } + else +#endif +#ifndef CONFIG_STM32_ILI9331_DISABLE + if (id == ILI9331_ID) + { + priv->type = LCD_TYPE_ILI9331; + stm32_lcd9331init(priv); + } + else +#endif +#ifndef CONFIG_STM32_ILI9325_DISABLE + if (id == ILI9325_ID) + { + priv->type = LCD_TYPE_ILI9325; + stm32_lcd9325init(priv, LCD_TYPE_ILI9325); + } + else +#endif +#ifndef CONFIG_STM32_ILI9328_DISABLE + if (id == ILI9328_ID) + { + priv->type = LCD_TYPE_ILI9328; + stm32_lcd9325init(priv, LCD_TYPE_ILI9328); + } + else +#endif +#ifndef CONFIG_STM32_ILI1505_DISABLE + if (id == 0x1505) + { + priv->type = LCD_TYPE_ILI1505; + stm32_lcd1505init(priv); + } + else +#endif +#ifndef CONFIG_STM32_ILI9919_DISABLE + if (id == 0x9919) + { + priv->type = LCD_TYPE_ILI9919; + stm32_lcd9919init(priv); + } + else +#endif + { + lcddbg("Unsupported LCD type\n"); + } + + lcddbg("LCD type: %d\n", priv->type); +} + + /************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: up_lcdinitialize + * + * Description: + * Initialize the LCD video hardware. The initial state of the LCD is fully + * initialized, display memory cleared, and the LCD ready to use, but with the power + * setting at 0 (full off). + * + ************************************************************************************/ + +int up_lcdinitialize(void) +{ + FAR struct stm32_dev_s *priv = &g_lcddev; + int i; + + lcdvdbg("Initializing\n"); + + /* Configure GPIO pins. The inialial state of priv->output is false, so + * we need to configure pins for output initially. + */ + + stm32_lcdoutput(priv); + + /* Configure control pins */ + + for (i = 0; i < NLCD_CONFIG; i++) + { + stm32_configgpio(g_lcdctrl[i]); + } + + /* Configure and enable LCD */ + + up_mdelay(50); + stm32_lcdinitialize(priv); + + /* Clear the display (setting it to the color 0=black) */ + + stm32_lcdclear(0); + + /* Turn the display off */ + + stm32_poweroff(priv); + return OK; +} + +/************************************************************************************ + * Name: up_lcdgetdev + * + * Description: + * Return a a reference to the LCD object for the specified LCD. This allows support + * for multiple LCD devices. + * + ************************************************************************************/ + +FAR struct lcd_dev_s *up_lcdgetdev(int lcddev) +{ + DEBUGASSERT(lcddev == 0); + return &g_lcddev.dev; +} + +/************************************************************************************ + * Name: up_lcduninitialize + * + * Description: + * Unitialize the LCD support + * + ************************************************************************************/ + +void up_lcduninitialize(void) +{ + FAR struct stm32_dev_s *priv = &g_lcddev; + + /* Put the LCD in the lowest possible power state */ + + stm32_poweroff(priv); + + /* Make sure that the LCD is not selected */ + + putreg32(1, LCD_CS_SET); +} + +/************************************************************************************ + * Name: stm32_lcdclear + * + * Description: + * This is a non-standard LCD interface just for the Shenzhou board. Because + * of the various rotations, clearing the display in the normal way by writing a + * sequences of runs that covers the entire display can be very slow. Here the + * display is cleared by simply setting all GRAM memory to the specified color. + * + ************************************************************************************/ + +void stm32_lcdclear(uint16_t color) +{ + FAR struct stm32_dev_s *priv = &g_lcddev; + uint32_t i = 0; + + stm32_setcursor(priv, 0, 0); + stm32_gramselect(priv); + + /* Make sure that we are configured for output */ + + stm32_lcdoutput(priv); + + /* Write the selected color into the entire GRAM memory */ + + putreg32(1, LCD_CS_CLEAR); + putreg32(1, LCD_RS_CLEAR); + for (i = 0; i < STM32_XRES * STM32_YRES; i++) + { + putreg32(1, LCD_WR_CLEAR); + putreg32((uint32_t)color, LCD_ODR); + putreg32(1, LCD_WR_SET); + } + + putreg32(1, LCD_CS_SET); +} + +#endif /* !HAVE_LCD */ diff --git a/nuttx/configs/shenzhou/src/up_ssd1289.c b/nuttx/configs/shenzhou/src/up_ssd1289.c index 42a0ffbd20..2b48c78ca7 100644 --- a/nuttx/configs/shenzhou/src/up_ssd1289.c +++ b/nuttx/configs/shenzhou/src/up_ssd1289.c @@ -68,11 +68,6 @@ * Pre-processor Definitions ************************************************************************************/ /* Configuration ********************************************************************/ - -#ifndef CONFIG_SSD1289_WRONLY -# warning "Only write access is supported; CONFIG_SSD1289_WRONLY should be defined" -#endif - /* Define CONFIG_DEBUG_LCD to enable detailed LCD debug output. Verbose debug must * also be enabled. */ @@ -87,6 +82,9 @@ # undef CONFIG_DEBUG_LCD #endif +#undef CONFIG_LCD_FASTCONFIG +#define CONFIG_LCD_FASTCONFIG 1 + /* Shenzhou LCD Hardware Definitions ************************************************/ /* Debug ****************************************************************************/ @@ -108,6 +106,9 @@ /* Helpers */ static void stm32_wrdata(uint16_t data); +#ifndef CONFIG_SSD1289_WRONLY +static uint16_t stm32_rddata(void); +#endif /* Low Level LCD access */ @@ -125,9 +126,9 @@ static void stm32_backlight(FAR struct ssd1289_lcd_s *dev, int power); ************************************************************************************/ /* TFT LCD * - * -- ---- -------------- ------------------------------------------------------------------- + * -- ---- -------------- ----------------------------------------------------------- * PN NAME SIGNAL NOTES - * -- ---- -------------- ------------------------------------------------------------------- + * -- ---- -------------- ----------------------------------------------------------- * 37 PB2 DATA_LE To TFT LCD (CN13, ping 28) * 96 PB9 F_CS To both the TFT LCD (CN13, pin 30) and to the W25X16 SPI FLASH * 34 PC5 TP_INT JP6. To TFT LCD (CN13) module (CN13, pin 26) @@ -198,14 +199,27 @@ static void stm32_backlight(FAR struct ssd1289_lcd_s *dev, int power); /* LCD GPIO configurations */ +#ifndef CONFIG_LCD_FASTCONFIG +static const uint32_t g_lcdout[16] = +{ + GPIO_LCD_D0OUT, GPIO_LCD_D1OUT, GPIO_LCD_D2OUT, GPIO_LCD_D3OUT, + GPIO_LCD_D4OUT, GPIO_LCD_D5OUT, GPIO_LCD_D6OUT, GPIO_LCD_D7OUT, + GPIO_LCD_D8OUT, GPIO_LCD_D9OUT, GPIO_LCD_D10OUT, GPIO_LCD_D11OUT, + GPIO_LCD_D12OUT, GPIO_LCD_D13OUT, GPIO_LCD_D14OUT, GPIO_LCD_D15OUT +}; + +static const uint32_t g_lcdin[16] = +{ + GPIO_LCD_D0IN, GPIO_LCD_D1IN, GPIO_LCD_D2IN, GPIO_LCD_D3IN, + GPIO_LCD_D4IN, GPIO_LCD_D5IN, GPIO_LCD_D6IN, GPIO_LCD_D7IN, + GPIO_LCD_D8IN, GPIO_LCD_D9IN, GPIO_LCD_D10IN, GPIO_LCD_D11IN, + GPIO_LCD_D12IN, GPIO_LCD_D13IN, GPIO_LCD_D14IN, GPIO_LCD_D15IN +}; +#endif + static const uint32_t g_lcdconfig[] = { - GPIO_LCD_D0, GPIO_LCD_D1, GPIO_LCD_D2, GPIO_LCD_D3, - GPIO_LCD_D4, GPIO_LCD_D5, GPIO_LCD_D6, GPIO_LCD_D7, - GPIO_LCD_D8, GPIO_LCD_D9, GPIO_LCD_D10, GPIO_LCD_D11, - GPIO_LCD_D12, GPIO_LCD_D13, GPIO_LCD_D14, GPIO_LCD_D15, - - GPIO_LCD_RS, GPIO_LCD_CS, GPIO_LCD_RD, GPIO_LCD_WR, + GPIO_LCD_RS, GPIO_LCD_CS, GPIO_LCD_RD, GPIO_LCD_WR, GPIO_LCD_LE, }; #define NLCD_CONFIG (sizeof(g_lcdconfig)/sizeof(uint32_t)) @@ -242,13 +256,41 @@ static FAR struct lcd_dev_s *g_ssd1289drvr; static void stm32_wrdata(uint16_t data) { + /* Make sure D0-D15 are configured as outputs */ + + stm32_lcdoutput(); + /* Latch the 16-bit LCD data and toggle the WR line */ - putreg32((uint32_t)data, LCD_DATA); putreg32(1, LCD_WR_CLEAR); + putreg32((uint32_t)data, LCD_ODR); putreg32(1, LCD_WR_SET); } +/************************************************************************************ + * Name: stm32_rddata + * + * Description: + * Latch data on D0-D15 and toggle the WR line. + * + ************************************************************************************/ + +#ifndef CONFIG_SSD1289_WRONLY +static uint16_t stm32_rddata(void); +{ + /* Make sure D0-D15 are configured as inputs */ + + stm32_lcdinput(); + + /* Toggle the RD line to latch the 16-bit LCD data */ + +#warning "Requires pins configured as inputs" + putreg32(1, LCD_RD_CLEAR); + putreg32(1, LCD_RD_SET); + return (uin16_t)getreg32(LCD_IDR); +} +#endif + /************************************************************************************ * Name: stm32_select * @@ -309,7 +351,13 @@ static void stm32_index(FAR struct ssd1289_lcd_s *dev, uint8_t index) #ifndef CONFIG_SSD1289_WRONLY static uint16_t stm32_read(FAR struct ssd1289_lcd_s *dev) { -#warning "Missing logic" + /* Set the RS signal */ + + putreg32(1, LCD_RS_CLR); + + /* And return the data */ + + return stm32_rddata(); } #endif @@ -345,6 +393,51 @@ static void stm32_backlight(FAR struct ssd1289_lcd_s *dev, int power) /* There is no software control over the backlight */ } +/************************************************************************************ + * Name: stm32_lcdinput + * + * Description: + * Config data lines for input operations. + * + ************************************************************************************/ + +static void stm32_lcdinput(void) +{ +#ifdef CONFIG_LCD_FASTCONFIG + putreg32(LCD_INPUT, LCD_CRL); + putreg32(LCD_INPUT, LCD_CRH); +#else + int i; + + /* Configure GPIO data lines as inputs */ + + for (i = 0; i < 16; i++) + { + stm32_configgpio(g_lcdin[i]); + } +#endif +} + +/************************************************************************************ + * Name: stm32_lcdoutput + * + * Description: + * Config data lines for output operations. + * + ************************************************************************************/ + +static void stm32_lcdoutput(void) +{ + int i; + + /* Configure GPIO data lines as outputs */ + + for (i = 0; i < 16; i++) + { + stm32_configgpio(g_lcdout[i]); + } +} + /************************************************************************************ * Public Functions ************************************************************************************/ @@ -371,6 +464,7 @@ int up_lcdinitialize(void) /* Configure GPIO pins */ + stm32_lcdoutput(); for (i = 0; i < NLCD_CONFIG; i++) { stm32_configgpio(g_lcdconfig[i]); From abbe998506e4ba49bbf6a9a9ae731b1eec521db6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 25 Sep 2012 21:35:02 +0200 Subject: [PATCH 37/56] ardrone in the air again (workaround: rate controller disabled) --- apps/ardrone_interface/ardrone_interface.c | 11 +++++++++-- apps/ardrone_interface/ardrone_motor_control.c | 9 ++++++--- .../multirotor_att_control_main.c | 16 +++++++++++++--- .../multirotor_attitude_control.c | 7 +++++++ 4 files changed, 35 insertions(+), 8 deletions(-) diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c index 8d77e7502e..f12f9cb474 100644 --- a/apps/ardrone_interface/ardrone_interface.c +++ b/apps/ardrone_interface/ardrone_interface.c @@ -239,14 +239,15 @@ int ardrone_interface_thread_main(int argc, char *argv[]) /* declare and safely initialize all structs */ struct vehicle_status_s state; - memset(&state, 0, sizeof(state)); + //memset(&state, 0, sizeof(state)); struct actuator_controls_s actuator_controls; - memset(&actuator_controls, 0, sizeof(actuator_controls)); + //memset(&actuator_controls, 0, sizeof(actuator_controls)); struct actuator_armed_s armed; armed.armed = false; /* subscribe to attitude, motor setpoints and system state */ int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + int state_sub = orb_subscribe(ORB_ID(vehicle_status)); int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); @@ -328,7 +329,13 @@ int ardrone_interface_thread_main(int argc, char *argv[]) * if in failsafe */ if (armed.armed && !armed.lockdown) { + + + + //printf("AMO_BEF: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",actuator_controls.control[0], actuator_controls.control[1], actuator_controls.control[2], actuator_controls.control[3]); + ardrone_mixing_and_output(ardrone_write, &actuator_controls); + } else { /* Silently lock down motor speeds to zero */ ardrone_write_motor_commands(ardrone_write, 0, 0, 0, 0); diff --git a/apps/ardrone_interface/ardrone_motor_control.c b/apps/ardrone_interface/ardrone_motor_control.c index 787db18773..cbf9600a59 100644 --- a/apps/ardrone_interface/ardrone_motor_control.c +++ b/apps/ardrone_interface/ardrone_motor_control.c @@ -368,6 +368,8 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls float yaw_control = actuators->control[2]; float motor_thrust = actuators->control[3]; + //printf("AMO: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",roll_control, pitch_control, yaw_control, motor_thrust); + const float min_thrust = 0.02f; /**< 2% minimum thrust */ const float max_thrust = 1.0f; /**< 100% max thrust */ const float scaling = 512.0f; /**< 100% thrust equals a value of 512 */ @@ -387,15 +389,16 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls if (motor_thrust <= min_thrust) { motor_thrust = min_thrust; output_band = 0.0f; - + //printf("0 silent\n"); } else if (motor_thrust < startpoint_full_control && motor_thrust > min_thrust) { output_band = band_factor * (motor_thrust - min_thrust); - + //printf("1 starting\n"); } else if (motor_thrust >= startpoint_full_control && motor_thrust < max_thrust - band_factor * startpoint_full_control) { output_band = band_factor * startpoint_full_control; - + //printf("2 working\n"); } else if (motor_thrust >= max_thrust - band_factor * startpoint_full_control) { output_band = band_factor * (max_thrust - motor_thrust); + //printf("3 full\n"); } //add the yaw, nick and roll components to the basic thrust //TODO:this should be done by the mixer diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 21c720096e..70b9d8e28d 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -76,7 +76,7 @@ __EXPORT int multirotor_att_control_main(int argc, char *argv[]); static bool thread_should_exit; static int mc_task; static bool motor_test_mode = false; -static struct actuator_controls_s actuators; + static orb_advert_t actuator_pub; /** @@ -86,6 +86,8 @@ static void *rate_control_thread_main(void *arg) { prctl(PR_SET_NAME, "mc rate control", getpid()); + struct actuator_controls_s actuators; + int gyro_sub = orb_subscribe(ORB_ID(sensor_gyro)); int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); @@ -125,8 +127,8 @@ static void *rate_control_thread_main(void *arg) gyro_lp[1] = gyro_report.y; gyro_lp[2] = gyro_report.z; - multirotor_control_rates(&rates_sp, gyro_lp, &actuators); - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); +// multirotor_control_rates(&rates_sp, gyro_lp, &actuators); +// orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); // } } } @@ -153,6 +155,8 @@ mc_thread_main(int argc, char *argv[]) struct vehicle_rates_setpoint_s rates_sp; memset(&rates_sp, 0, sizeof(rates_sp)); + struct actuator_controls_s actuators; + /* subscribe to attitude, motor setpoints and system state */ int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); @@ -228,6 +232,7 @@ mc_thread_main(int argc, char *argv[]) /* set yaw rate */ rates_sp.yaw = manual.yaw; att_sp.thrust = manual.throttle; + //printf("Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",att_sp.roll_body, att_sp.pitch_body, att_sp.yaw_body, att_sp.thrust); att_sp.timestamp = hrt_absolute_time(); /* STEP 2: publish the result to the vehicle actuators */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); @@ -269,10 +274,15 @@ mc_thread_main(int argc, char *argv[]) /* run attitude controller */ if (state.flag_control_attitude_enabled && !state.flag_control_rates_enabled) { multirotor_control_attitude(&att_sp, &att, NULL, &actuators); +// printf("publish actuator\n"); + +// printf("MAC_PUB: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",actuators.control[0], actuators.control[1], actuators.control[2], actuators.control[3]); + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); } else if (state.flag_control_attitude_enabled && state.flag_control_rates_enabled) { multirotor_control_attitude(&att_sp, &att, &rates_sp, NULL); orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); +// printf("publish attitude\n"); } diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c index 2129915d12..c25e96856a 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -312,6 +312,10 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s actuators->control[3] = motor_thrust; } +// if(motor_skip_counter%20 == 0) +// printf("MAC: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",actuators->control[0], actuators->control[1], actuators->control[2], actuators->control[3]); + + // XXX change yaw rate to yaw pos controller if (rates_sp) { rates_sp->roll = roll_control; @@ -320,5 +324,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s rates_sp->thrust = motor_thrust; } +// if(motor_skip_counter%20 == 0) +// printf("Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",rates_sp->roll, rates_sp->pitch, rates_sp->yaw, rates_sp->thrust); + motor_skip_counter++; } From fd3df782b1ceca6bc7f36817639c2a9b80560c02 Mon Sep 17 00:00:00 2001 From: patacongo Date: Tue, 25 Sep 2012 21:15:02 +0000 Subject: [PATCH 38/56] Add more LCD-related Kconfig logic; Create a Kconfig file for NxWidgets git-svn-id: http://svn.code.sf.net/p/nuttx/code/trunk@5189 42af7a65-404d-4744-a932-0658087f49c3 --- NxWidgets/ChangeLog.txt | 3 + NxWidgets/Kconfig | 569 ++++++++++++++++++++ NxWidgets/libnxwidgets/include/nxconfig.hxx | 2 +- NxWidgets/nxwm/include/nxwmconfig.hxx | 12 +- nuttx/ChangeLog | 5 +- nuttx/Kconfig | 7 + nuttx/configs/shenzhou/Kconfig | 72 +++ nuttx/configs/shenzhou/src/up_lcd.c | 3 +- nuttx/drivers/lcd/Kconfig | 15 + nuttx/graphics/Kconfig | 14 +- 10 files changed, 690 insertions(+), 12 deletions(-) create mode 100644 NxWidgets/Kconfig diff --git a/NxWidgets/ChangeLog.txt b/NxWidgets/ChangeLog.txt index cdd26cb896..e0a4c9b2e1 100644 --- a/NxWidgets/ChangeLog.txt +++ b/NxWidgets/ChangeLog.txt @@ -163,3 +163,6 @@ * UnitTests/*/main.cxx: Change entry point name to be consistent with with entry point naming conventions introduced in NuttX 6.22. +* Kconfig: Added a mconfig configuration file. Eventually, NxWidgets + needs to get hooked into the NuttX mconf configuration. Still not + exactly sure how to do that. diff --git a/NxWidgets/Kconfig b/NxWidgets/Kconfig new file mode 100644 index 0000000000..c89b4936f1 --- /dev/null +++ b/NxWidgets/Kconfig @@ -0,0 +1,569 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# + +menuconfig NxWIDGETS + bool "Enable NxWidgets" + default n + depends on NX && HAVE_CXX + ---help--- + Enable support for NxWidgets + +if NxWIDGETS +comment "NX Server/Device Configuration" + +config NXWIDGETS_DEVNO + int "LCD Device Number" + default 0 + ---help--- + LCD device number (in case there are more than one LCDs connected). + Default: 0 + +config NXWIDGETS_VPLANE + int "Plane Number" + default 0 + ---help--- + Only a single video plane is supported. Default: 0 + +config NXWIDGETS_SERVERPRIO + int "NX Server priority" + default 51 + ---help--- + Priority of the NX server. This applies only if NX is configured in + multi-user mode (NX_MULTIUSER=y). Default: 51. + + NOTE: Of the three priority definitions here, NXWIDGETS_SERVERPRIO + should have the highest priority to avoid data overrun race conditions. + Such errors would most likely appear as duplicated rows of data on the + display. + +config NXWIDGETS_SERVERSTACK + int "NX Server Stack Size" + default 2048 + ---help--- + NX server thread stack size (in multi-user mode). Default 2048 + +config NXWIDGETS_CLIENTPRIO + int "NX Client Priority" + default 50 + ---help--- + The thread that calls CNxServer::connect() will be re-prioritized to + this priority. This applies only if NX is configured in multi-user + mode (NX_MULTIUSER=y). Default: 50 + +config NXWIDGETS_LISTENERPRIO + int "NX Listener Priority" + default 50 + ---help--- + Priority of the NX event listener thread. This applies only if NX + is configured in multi-user mode (NX_MULTIUSER=y). Default: 50 + +config NXWIDGETS_LISTENERSTACK + int "NX Listener Stack Size" + default 2048 + ---help--- + NX listener thread stack size (in multi-user mode). Default 2048 + +config NXWIDGETS_EXTERNINIT + bool "Extern LCD Initialization" + ---help--- + Define to support external display initialization. + +config NXWIDGET_EVENTWAIT + bool "Event Waiting" + default n + ---help--- + Build in support for external window event, modal loop management + logic. This includes methods to wait for windows events to occur + so that looping logic can sleep until something interesting happens + with the window. + +comment "NXWidget Configuration" + +config NXWIDGETS_BPP + int "BPP" + ---help--- + Supported bits-per-pixel {8, 16, 24, 32}. Default: The smallest + BPP configuration supported by NX. + +config NXWIDGETS_SIZEOFCHAR + int "Size of a character (1 or 2 bytes) + range 1 2 + ---help--- + Size of character {1 or 2 bytes}. Default Determined by + NXWIDGETS_SIZEOFCHAR + +comment "NXWidget Default Values" + +config NXWIDGETS_DEFAULT_FONTID + int "Default Font ID" + ---help--- + Default font ID. Default: NXFONT_DEFAULT + +config NXWIDGETS_TNXARRAY_INITIALSIZE + int "Initial Size of Dynamic Arrays" + default 16 + ---help--- + Default dynamic array size (in entries). Default: 16 + +config NXWIDGETS_TNXARRAY_SIZEINCREMENT + int "Dyanamic Array Reallocation Size Increment" + default 8 + ---help--- + Default dynamic array realloctino increment (in entries). Default: 8 + +config NXWIDGETS_DEFAULT_BACKGROUNDCOLOR + hex "Normal Background Color" + ---help--- + Normal background color. Default: RGB(160,160,160) + +config NXWIDGETS_DEFAULT_SELECTEDBACKGROUNDCOLOR + hex "Selected Background Color" + ---help--- + Default selected background color. Default: RGB(120,192,192) + +config NXWIDGETS_DEFAULT_SHINEEDGECOLOR + hex "Shiny Edge Color" + ---help--- + Shiny side boarder color. Default: RGB(248,248,248) + +config NXWIDGETS_DEFAULT_SHADOWEDGECOLOR + hex "Shadow Edge Color" + ---help--- + Shadowed side border color. Default: RGB(0,0,0) + +config NXWIDGETS_DEFAULT_HIGHLIGHTCOLOR + hex "Highlight Color" + ---help--- + Highlight color. Default: RGB(192,192,192) + +config NXWIDGETS_DEFAULT_DISABLEDTEXTCOLOR + hex "Disabled Text Color" + ---help--- + Text color on a disabled widget: Default: RGB(192,192,192) + +config NXWIDGETS_DEFAULT_ENABLEDTEXTCOLOR + hex "Enabled Text Color" + ---help--- + Text color on a enabled widget. Default: RGB(248,248,248) + +config NXWIDGETS_DEFAULT_SELECTEDTEXTCOLOR + hex "Selected Text Color" + ---help--- + Text color on a selected widget. Default: RGB(0,0,0) + +config NXWIDGETS_DEFAULT_FONTCOLOR + hex "Default Font Color" + ---help--- + Default font color. Default: RGB(255,255,255) + +config NXWIDGETS_TRANSPARENT_COLOR + hex "Transparent Color" + ---help--- + Transparent color. Default: RGB(0,0,0) + +comment "Keypad behavior" + +config NXWIDGETS_FIRST_REPEAT_TIME + int "First Repeat Time" + default 500 + ---help--- + Time taken before a key starts repeating (in milliseconds). Default: 500 + +config NXWIDGETS_CONTINUE_REPEAT_TIME + int "Continue Repeat Time" + default 200 + ---help--- + Time taken before a repeating key repeats again (in milliseconds). + Default: 200 + +config NXWIDGETS_DOUBLECLICK_TIME + int "Double Click Time" + default 350 + ---help--- + Left button release-press time for double click (in milliseconds). + Default: 350 + +config NXWIDGETS_KBDBUFFER_SIZE + int "Keybard Buffer Size" + default 16 + ---help--- + Size of incoming character buffer, i.e., the maximum number of + characters that can be entered between NX polling cycles without + losing data. + +config NXWIDGETS_CURSORCONTROL_SIZE + int "Cursor Control Buffer Size" + default 4 + ---help--- + Size of incoming cursor control buffer, i.e., the maximum number + of cursor controls that can between entered by NX polling cycles + without losing data. Default: 4 + +endif + +menuconfig NxWM + bool "Enable NxWM" + default n + depends on NxWIDGETS && NX_MULTIUSER + ---help--- + Enable support for the NuttX Tiny Window Manager (NxWM) + +if NxWM +comment "General settings" + +config NXWM_DEFAULT_FONTID + int "Font ID" + ---help--- + The NxWM default font ID. Default: NXFONT_DEFAULT + +comment "Color configuration" + +config NXWM_DEFAULT_BACKGROUNDCOLOR + hex "Background Color" + ---help--- + Normal background color. Default: RGB(148,189,215) + +config NXWM_DEFAULT_SELECTEDBACKGROUNDCOLOR + hex "Normal Background Color" + ---help--- + Select background color. Default: RGB(206,227,241) + +config NXWM_DEFAULT_SHINEEDGECOLOR + hex "Shiny Edge Color" + ---help--- + Color of the bright edge of a border. Default: RGB(255,255,255) + +config NXWM_DEFAULT_SHADOWEDGECOLOR + hex "Shadow Edge Color" + ---help--- + Color of the shadowed edge of a border. Default: RGB(0,0,0) + +config NXWM_DEFAULT_FONTCOLOR + hex "Default Font Color" + ---help--- + Default fong color. Default: RGB(0,0,0) + +config NXWM_TRANSPARENT_COLOR + hex "Transparent Color" + ---help--- + The "transparent" color. Default: RGB(0,0,0) + +comment "Horizontal and vertical spacing of icons in the task bar" + +config NXWM_TASKBAR_VSPACING + int "Vertical Spacing" + default 2 + ---help--- + Vertical spacing. Default: 2 pixels + +config NXWM_TASKBAR_HSPACING + int "Horizontal Spacing" + default 2 + ---help--- + Horizontal spacing. Default: 2 rows + +choice NXWM_TASKBAR_LOCATION + prompt "Taskbar Location" + default NXWM_TASKBAR_TOP + +config NXWM_TASKBAR_TOP + bool "Top" + ---help--- + Task bar is at the top of the display + +config NXWM_TASKBAR_BOTTOM + bool "Bottom" + ---help--- + Task bar is at the bottom of the display + +config NXWM_TASKBAR_LEFT + bool "Left" + ---help--- + Task bar is on the left side of the display + +config NXWM_TASKBAR_RIGHT + bool "Right" + ---help--- + Task bar is on the right side of the display + +endchoice + +config NXWM_TASKBAR_WIDTH + int "Taskbar Width" + ---help--- + Task bar thickness (either vertical or horizontal). Default: 25 + 2*spacing + +comment "Tool Bar Configuration" + +config NXWM_TOOLBAR_HEIGHT + int "Toolbar Height" + ---help--- + The height of the tool bar in each application window. At present, + all icons are 21 pixels in height and, hence, require a task bar of + at least that size. + +comment "Background Image" + +config NXWM_BACKGROUND_IMAGE + string "Background Image" + ---help--- + The name of the image to use in the background window. Default: + NXWidgets::g_nuttxBitmap + +comment "Start Window Configuration" + +comment "Horizontal and vertical spacing of icons in the task bar" + +config NXWM_STARTWINDOW_VSPACING + int "Vertical Spacing" + default 4 + ---help--- + Vertical spacing. Default: 4 pixels + +config NXWM_STARTWINDOW_HSPACING + int "Horizontal Spacing" + default 4 + ---help--- + Horizontal spacing. Default: 4 rows + +config NXWM_STARTWINDOW_ICON + string "StartWindow Icon" + ---help--- + The glyph to use as the start window icon. Default: NxWM::g_playBitmap + +config NXWM_STARTWINDOW_MQNAME + string "Message Queue Name" + default "/dev/nxwm" + ---help--- + The well known name of the message queue. Used to communicated from + CWindowMessenger to the start window thread. Default: "/dev/nxwm" + +config NXWM_STARTWINDOW_MXMSGS + int "Max Messages" + default 32 + ---help--- + The maximum number of messages to queue before blocking. Defualt 32 + +config NXWM_STARTWINDOW_MXMPRIO + int "Message Priority" + default 42 + ---help--- + The message priority. Default: 42. + +config NXWM_STARTWINDOW_PRIO + int "StartWindow Task Priority" + default 50 + ---help--- + Priority of the StartWindow task. Default: 50. + + NOTE: This priority should be less than NXWIDGETS_SERVERPRIO or else + there may be data overrun errors. Such errors would most likely appear + as duplicated rows of data on the display. + +config NXWM_STARTWINDOW_STACKSIZE + int "StartWindow Task Stack Size" + default 2048 + ---help--- + The stack size to use when starting the StartWindow task. Default: + 2048 bytes. + +comment "NxConsole Window Configuration" + +config NXWM_NXCONSOLE_PRIO + int "NxConsole Task Priority" + default 50 + ---help--- + Priority of the NxConsole task. Default: 50. + + NOTE: This priority should be less than NXWIDGETS_SERVERPRIO or + else there may be data overrun errors. Such errors would most likely + appear as duplicated rows of data on the display. + +config NXWM_NXCONSOLE_STACKSIZE + int "NxConsole Task Stack Size" + default 2048 + ---help--- + The stack size to use when starting the NxConsole task. Default: + 2048 bytes. + +config NXWM_NXCONSOLE_WCOLOR + hex "NxConsole Background Color" + ---help--- + The color of the NxConsole window background. Default: + RGB(192,192,192) + +config NXWM_NXCONSOLE_FONTCOLOR + hex "NxConsole Font Color" + ---help--- + The color of the fonts to use in the NxConsole window. + Default: RGB(0,0,0) + +config NXWM_NXCONSOLE_FONTID + int "NxConsole Font ID" + ---help--- + The ID of the font to use in the NxConsole window. Default: + NXWM_DEFAULT_FONTID + +config NXWM_NXCONSOLE_ICON + string "NxConsole Icon" + ---help--- + The glyph to use as the NxConsole icon. Default: NxWM::g_cmdBitmap + +config NXWM_TOUCHSCREEN + bool "Touchscreen Support" + default y if INPUT + default n if !INPUT + ---help--- + Define to build in touchscreen support. + +if NXWM_TOUCHSCREEN +comment "Touchscreen device settings" + +config NXWM_TOUCHSCREEN_DEVNO + int "Touchscreen Device Number" + default 0 + ---help--- + Touchscreen device minor number, i.e., the N in /dev/inputN. + Default: 0 + +config NXWM_TOUCHSCREEN_DEVPATH + string "Touchscreen Device Path" + default "/dev/input0" + ---help--- + The full path to the touchscreen device. Default: "/dev/input0" + +config NXWM_TOUCHSCREEN_SIGNO + int "Touchscreen Signal Number" + default 5 + ---help--- + The realtime signal used to wake up the touchscreen listener + thread. Default: 5 + +config NXWM_TOUCHSCREEN_LISTENERPRIO + int "Touchscreen Listener Task Priority" + default 50 + ---help--- + Priority of the touchscreen listener thread. Default: 50 + +config NXWM_TOUCHSCREEN_LISTENERSTACK + int "Touchscreen Listener Task Stack Size" + ---help--- + Touchscreen listener thread stack size. Default 1024 + +endif + +config NXWM_KEYBOARD + bool "Keyboard Support" + default n + ---help--- + Define to build in touchscreen support. + +if NXWM_KEYBOARD +comment "Keyboard device settings" + +config NXWM_KEYBOARD_DEVPATH + string "Keyboard Device Path" + default "/dev/console" + ---help--- + The full path to the touchscreen device. Default: "/dev/console" + +config NXWM_KEYBOARD_SIGNO + int "Keyboard Task Signal Number" + default 6 + ---help--- + The realtime signal used to wake up the touchscreen listener thread. + Default: 6 + +config NXWM_KEYBOARD_BUFSIZE + int "Keyboard Buffer Size" + default 16 + ---help--- + The size of the keyboard read data buffer. Default: 16 + +config NXWM_KEYBOARD_LISTENERPRIO + int "Keyboard Listener Task Priority" + default 50 + ---help--- + Priority of the touchscreen listener thread. Default: 50 + +config NXWM_KEYBOARD_LISTENERSTACK + int "Keyboard Listener Task Stack Size" + default 2048 + ---help--- + Keyboard listener thread stack size. Default: 1024 + +endif + +comment "Calibration display settings" + +config NXWM_CALIBRATION_BACKGROUNDCOLOR + hex "Background Color" + ---help--- + The background color of the touchscreen calibration display. + Default: Same as NXWM_DEFAULT_BACKGROUNDCOLOR. + +config NXWM_CALIBRATION_LINECOLOR + hex "Line Color" + ---help--- + The color of the lines used in the touchscreen calibration display. + Default: RGB(0, 0, 128) (dark blue) + +config NXWM_CALIBRATION_CIRCLECOLOR + hex "Normal Circle Color" + ---help--- + The color of the circle in the touchscreen calibration display. + Default: RGB(255, 255, 255) (white) + +config NXWM_CALIBRATION_TOUCHEDCOLOR + hex "Touched Circle Color" + ---help--- + The color of the circle in the touchscreen calibration display after + the touch is recorder. Default: RGB(255, 255, 96) (very light yellow) + +config NXWM_CALIBRATION_ICON + string "Callibration Icon" + ---help--- + The ICON to use for the touchscreen calibration application. Default: + NxWM::g_calibrationBitmap + +config NXWM_CALIBRATION_SIGNO + int "Calibration Signal Number" + default 5 + ---help--- + The realtime signal used to wake up the touchscreen calibration + thread. Default: 5 + +config NXWM_CALIBRATION_LISTENERPRIO + int "Calibration Task Priority" + default 50 + ---help--- + Priority of the calibration listener thread. Default: 50 + +config NXWM_CALIBRATION_LISTENERSTACK + int "Calibration Task Stack Size" + default 2048 + ---help--- + Calibration listener thread stack size. Default 2048 + +comment "Calibration display settings" + +config NXWM_HEXCALCULATOR_BACKGROUNDCOLOR + hex "Calculator Background Color" + ---help--- + The background color of the calculator display. Default: Same + as NXWM_DEFAULT_BACKGROUNDCOLOR + +config NXWM_HEXCALCULATOR_ICON + string "Calculator Icon" + ---help--- + The ICON to use for the hex calculator application. Default: + NxWM::g_calculatorBitmap + +config NXWM_HEXCALCULATOR_FONTID + int "Calculator Font ID" + ---help--- + The font used with the calculator. Default: NXWM_DEFAULT_FONTID + +endif diff --git a/NxWidgets/libnxwidgets/include/nxconfig.hxx b/NxWidgets/libnxwidgets/include/nxconfig.hxx index 699686822e..67d359622c 100644 --- a/NxWidgets/libnxwidgets/include/nxconfig.hxx +++ b/NxWidgets/libnxwidgets/include/nxconfig.hxx @@ -117,7 +117,7 @@ * Default: MKRGB(192,192,192) * CONFIG_NXWIDGETS_DEFAULT_ENABLEDTEXTCOLOR - Text color on a enabled widget: * Default: MKRGB(248,248,248) - * CONFIG_NXWIDGETS_DEFAULT_DISABLEDTEXTCOLOR - Text color on a selected widget: + * CONFIG_NXWIDGETS_DEFAULT_SELECTEDTEXTCOLOR - Text color on a selected widget: * Default: MKRGB(0,0,0) * CONFIG_NXWIDGETS_DEFAULT_FONTCOLOR - Default font color: Default: * MKRGB(255,255,255) diff --git a/NxWidgets/nxwm/include/nxwmconfig.hxx b/NxWidgets/nxwm/include/nxwmconfig.hxx index 578fc5be83..0c43125bbd 100644 --- a/NxWidgets/nxwm/include/nxwmconfig.hxx +++ b/NxWidgets/nxwm/include/nxwmconfig.hxx @@ -270,13 +270,13 @@ * CONFIG_NXWM_STARTWINDOW_MXMSGS - The maximum number of messages to queue * before blocking. Defualt 32 * CONFIG_NXWM_STARTWINDOW_MXMPRIO - The message priority. Default: 42. - * CONFIG_NXWM_STARTWINDOW_PRIO - Priority of the NxConsole task. Default: + * CONFIG_NXWM_STARTWINDOW_PRIO - Priority of the StartWindoW task. Default: * SCHED_PRIORITY_DEFAULT. NOTE: This priority should be less than * CONFIG_NXWIDGETS_SERVERPRIO or else there may be data overrun errors. * Such errors would most likely appear as duplicated rows of data on the * display. * CONFIG_NXWM_STARTWINDOW_STACKSIZE - The stack size to use when starting the - * NxConsole task. Default: 2048 bytes. + * StartWindow task. Default: 2048 bytes. */ #ifndef CONFIG_NXWM_STARTWINDOW_VSPACING @@ -421,7 +421,7 @@ /** * Keyboard device settings * - * CONFIG_NXWM_KEYBOARD_DEVNO - The full path to the touchscreen device. + * CONFIG_NXWM_KEYBOARD_DEVPATH - The full path to the keyboard device. * Default: "/dev/console" * CONFIG_NXWM_KEYBOARD_SIGNO - The realtime signal used to wake up the * touchscreen listener thread. Default: 6 @@ -464,16 +464,16 @@ * touchscreen calibration display. Default: MKRGB(0, 0, 128) (dark blue) * CONFIG_NXWM_CALIBRATION_CIRCLECOLOR - The color of the circle in the * touchscreen calibration display. Default: MKRGB(255, 255, 255) (white) - * CONFIG_NXWM_CALIBRATION_CIRCLECOLOR - The color of the circle in the + * CONFIG_NXWM_CALIBRATION_TOUCHEDCOLOR - The color of the circle in the * touchscreen calibration display after the touch is recorder. Default: * MKRGB(255, 255, 96) (very light yellow) * CONFIG_NXWM_CALIBRATION_ICON - The ICON to use for the touchscreen * calibration application. Default: NxWM::g_calibrationBitmap * CONFIG_NXWM_CALIBRATION_SIGNO - The realtime signal used to wake up the * touchscreen calibration thread. Default: 5 - * CONFIG_NXWM_CALIBRATION_LISTENERPRIO - Priority of the touchscreen listener + * CONFIG_NXWM_CALIBRATION_LISTENERPRIO - Priority of the calibration listener * thread. Default: SCHED_PRIORITY_DEFAULT - * CONFIG_NXWM_CALIBRATION_LISTENERSTACK - Touchscreen listener thread stack + * CONFIG_NXWM_CALIBRATION_LISTENERSTACK - Calibration listener thread stack * size. Default 2048 */ diff --git a/nuttx/ChangeLog b/nuttx/ChangeLog index 895f2844fc..56929ad2f5 100644 --- a/nuttx/ChangeLog +++ b/nuttx/ChangeLog @@ -3396,4 +3396,7 @@ will need a completely need bit-banging interface; this initial check-in is only for the framework. * configs/shenzhou/src/up_ssd1289.c: Bit-banging driver is - code complete. \ No newline at end of file + code complete. + * configs/shenzhou/src/up_lcd.c: Oops. Shenzhou LCD does not + have an SSD1289 controller. Its an ILI93xx. Ported the + STM3240G-EVAL ILI93xx driver to work on the Shenzhou board. diff --git a/nuttx/Kconfig b/nuttx/Kconfig index 520f09ebff..86e8849bc8 100644 --- a/nuttx/Kconfig +++ b/nuttx/Kconfig @@ -286,6 +286,13 @@ config DEBUG_GRAPHICS ---help--- Enable NX graphics debug output (disabled by default) +config DEBUG_LCD + bool "Enable low-leve LCD debug output" + default n + depends on LCD + ---help--- + Enable low level debug output from the LCD driver (disabled by default) + config DEBUG_I2C bool "Enable I2C debug output" default n diff --git a/nuttx/configs/shenzhou/Kconfig b/nuttx/configs/shenzhou/Kconfig index 92d2940995..bfd67abdfb 100644 --- a/nuttx/configs/shenzhou/Kconfig +++ b/nuttx/configs/shenzhou/Kconfig @@ -4,4 +4,76 @@ # if ARCH_BOARD_SHENZHOU + +if LCD +menu "LCD Controller Selection" + +comment "Disable Unused LCD Controllers" + +config STM32_ILI1505_DISABLE + bool "Disable ILI1505" + default n + ---help--- + This may be defined to disable support for the ILI1505 LCD controller + You might want to eliminate unused LCD controll support in order to + reduce the FLASH footprint. + +config STM32_ILI9300_DISABLE + bool "Disable ILI9300" + default n + ---help--- + This may be defined to disable support for the ILI9300 LCD controller + You might want to eliminate unused LCD controll support in order to + reduce the FLASH footprint. + +config STM32_ILI9320_DISABLE + bool "Disable ILI9320" + default n + ---help--- + This may be defined to disable support for the ILI9320 LCD controller + You might want to eliminate unused LCD controll support in order to + reduce the FLASH footprint. + +config STM32_ILI9321_DISABLE + bool "Disable ILI9321" + default n + ---help--- + This may be defined to disable support for the ILI9321 LCD controller + You might want to eliminate unused LCD controll support in order to + reduce the FLASH footprint. + +config STM32_ILI9325_DISABLE + bool "Disable ILI9325" + default n + ---help--- + This may be defined to disable support for the ILI9325 LCD controller + You might want to eliminate unused LCD controll support in order to + reduce the FLASH footprint. + +config STM32_ILI9328_DISABLE + bool "Disable ILI9328" + default n + ---help--- + This may be defined to disable support for the ILI9328 LCD controller + You might want to eliminate unused LCD controll support in order to + reduce the FLASH footprint. + +config STM32_ILI9331_DISABLE + bool "Disable ILI9331" + default n + ---help--- + This may be defined to disable support for the ILI9331 LCD controller + You might want to eliminate unused LCD controll support in order to + reduce the FLASH footprint. + +config STM32_ILI9919_DISABLE + bool "Disable ILI9919" + default n + ---help--- + This may be defined to disable support for the ILI9919 LCD controller + You might want to eliminate unused LCD controll support in order to + reduce the FLASH footprint. + +endmenu +endif endif diff --git a/nuttx/configs/shenzhou/src/up_lcd.c b/nuttx/configs/shenzhou/src/up_lcd.c index a58b795c42..1de2a7d4bc 100644 --- a/nuttx/configs/shenzhou/src/up_lcd.c +++ b/nuttx/configs/shenzhou/src/up_lcd.c @@ -135,8 +135,7 @@ * Pre-processor Definitions ************************************************************************************/ /* Configuration **********************************************************************/ -/* - * CONFIG_STM32_ILI1505_DISABLE may be defined to disable the LCD_ILI1505 +/* CONFIG_STM32_ILI1505_DISABLE may be defined to disable the LCD_ILI1505 * CONFIG_STM32_ILI9300_DISABLE may be defined to disable the LCD_ILI9300 * CONFIG_STM32_ILI9320_DISABLE may be defined to disable the LCD_ILI9320 * CONFIG_STM32_ILI9321_DISABLE may be defined to disable the LCD_ILI9321 diff --git a/nuttx/drivers/lcd/Kconfig b/nuttx/drivers/lcd/Kconfig index 081a79c89d..851263d279 100644 --- a/nuttx/drivers/lcd/Kconfig +++ b/nuttx/drivers/lcd/Kconfig @@ -2,6 +2,21 @@ # For a description of the syntax of this configuration file, # see misc/tools/kconfig-language.txt. # + +config LCD_NOGETRUN + bool "Write-only LCD" + default n + ---help--- + Many LCD hardware interfaces provide only minimal graphics capability. In + particulary, many simple LCD interfaces are write only. That is we, can + write graphics data to the LCD device memory, but we cannot read it back. + If the LCD hardware does not support reading the graphics memory, then + this option should be defined so that the NX layer can taking alternative + measures when the LCD is not readable. For example, if the LCD is not + readable, then NX will not attempt to support transparency. + + See also NX_WRITEONLY in the graphics support menu. + config LCD_MAXCONTRAST int "LCD maximum contrast" default 63 if NOKIA6100_S1D15G10 diff --git a/nuttx/graphics/Kconfig b/nuttx/graphics/Kconfig index 1ae387605b..18b1e1ab97 100644 --- a/nuttx/graphics/Kconfig +++ b/nuttx/graphics/Kconfig @@ -11,6 +11,16 @@ config NX if NX +config NX_LCDDRIVER + bool "LCD driver" + default y + depends on LCD + ---help--- + By default, the NX graphics system uses the frame buffer driver interface + defined in include/nuttx/fb.h. However, if LCD is support is enabled, + this this option is provide to select, instead, the LCD driver interface + defined in include/nuttx/lcd/lcd.h. + config NX_NPLANES int "Number of Color Planes" default 1 @@ -21,8 +31,8 @@ config NX_NPLANES config NX_WRITEONLY bool "Write-only Graphics Device" - default y if NX_LCDDRIVER && NX_LCDDRIVER - default n if !NX_LCDDRIVER || !NX_LCDDRIVER + default y if NX_LCDDRIVER && LCD_NOGETRUN + default n if !NX_LCDDRIVER || !LCD_NOGETRUN ---help--- Define if the underlying graphics device does not support read operations. Automatically defined if NX_LCDDRIVER and LCD_NOGETRUN are From 2be7847efe154ad3c6c0f4e869d9142e72d8a544 Mon Sep 17 00:00:00 2001 From: patacongo Date: Tue, 25 Sep 2012 22:04:51 +0000 Subject: [PATCH 39/56] Hook in NxWidgets configuration logic; Add a untested ADS7843E touchscreen support for the Shenzhou board; Complete the Shenzhou NxWM configuration (also untested). git-svn-id: http://svn.code.sf.net/p/nuttx/code/trunk@5190 42af7a65-404d-4744-a932-0658087f49c3 --- NxWidgets/Kconfig | 24 +- NxWidgets/README.txt | 6 + NxWidgets/libnxwidgets/include/nxconfig.hxx | 12 +- apps/ChangeLog.txt | 5 + apps/Kconfig | 4 + apps/NxWidgets/Kconfig | 575 +++++++++ apps/NxWidgets/README.txt | 11 + nuttx/ChangeLog | 7 + nuttx/configs/shenzhou/nxwm/defconfig | 1196 +++++++++++++++++++ nuttx/configs/shenzhou/src/Makefile | 4 + nuttx/configs/shenzhou/src/up_touchscreen.c | 292 +++++ 11 files changed, 2121 insertions(+), 15 deletions(-) create mode 100644 apps/NxWidgets/Kconfig create mode 100644 apps/NxWidgets/README.txt create mode 100644 nuttx/configs/shenzhou/nxwm/defconfig create mode 100644 nuttx/configs/shenzhou/src/up_touchscreen.c diff --git a/NxWidgets/Kconfig b/NxWidgets/Kconfig index c89b4936f1..1d7852507c 100644 --- a/NxWidgets/Kconfig +++ b/NxWidgets/Kconfig @@ -3,14 +3,14 @@ # see misc/tools/kconfig-language.txt. # -menuconfig NxWIDGETS +menuconfig NXWIDGETS bool "Enable NxWidgets" default n depends on NX && HAVE_CXX ---help--- Enable support for NxWidgets -if NxWIDGETS +if NXWIDGETS comment "NX Server/Device Configuration" config NXWIDGETS_DEVNO @@ -88,7 +88,7 @@ config NXWIDGETS_BPP BPP configuration supported by NX. config NXWIDGETS_SIZEOFCHAR - int "Size of a character (1 or 2 bytes) + int "Size of a character (1 or 2 bytes)" range 1 2 ---help--- Size of character {1 or 2 bytes}. Default Determined by @@ -116,12 +116,12 @@ config NXWIDGETS_TNXARRAY_SIZEINCREMENT config NXWIDGETS_DEFAULT_BACKGROUNDCOLOR hex "Normal Background Color" ---help--- - Normal background color. Default: RGB(160,160,160) + Normal background color. Default: RGB(148,189,215) config NXWIDGETS_DEFAULT_SELECTEDBACKGROUNDCOLOR hex "Selected Background Color" ---help--- - Default selected background color. Default: RGB(120,192,192) + Default selected background color. Default: RGB(206,227,241) config NXWIDGETS_DEFAULT_SHINEEDGECOLOR hex "Shiny Edge Color" @@ -131,7 +131,7 @@ config NXWIDGETS_DEFAULT_SHINEEDGECOLOR config NXWIDGETS_DEFAULT_SHADOWEDGECOLOR hex "Shadow Edge Color" ---help--- - Shadowed side border color. Default: RGB(0,0,0) + Shadowed side border color. Default: RGB(35,58,73) config NXWIDGETS_DEFAULT_HIGHLIGHTCOLOR hex "Highlight Color" @@ -203,14 +203,14 @@ config NXWIDGETS_CURSORCONTROL_SIZE endif -menuconfig NxWM +menuconfig NXWM bool "Enable NxWM" default n - depends on NxWIDGETS && NX_MULTIUSER + depends on NXWIDGETS && NX_MULTIUSER ---help--- Enable support for the NuttX Tiny Window Manager (NxWM) -if NxWM +if NXWM comment "General settings" config NXWM_DEFAULT_FONTID @@ -218,6 +218,12 @@ config NXWM_DEFAULT_FONTID ---help--- The NxWM default font ID. Default: NXFONT_DEFAULT +config NXWM_UNITTEST + bool "NxWM Unit Test" + default n + ---help--- + Enable Hooks for the NxWM Unit Test + comment "Color configuration" config NXWM_DEFAULT_BACKGROUNDCOLOR diff --git a/NxWidgets/README.txt b/NxWidgets/README.txt index 7bb16820c5..81f100d3d1 100644 --- a/NxWidgets/README.txt +++ b/NxWidgets/README.txt @@ -53,6 +53,12 @@ license. See the COPYING file for details. Directory Structure =================== +Kconfig + + This is a Kconfig file that should be provided at apps/NxWidgets/Kconfig. + When copied to that location, it will be used by the NuttX configuration + systems to configure settings for NxWidgets and NxWM + libnxwidgets The source code, header files, and build environment for NxWidgets is diff --git a/NxWidgets/libnxwidgets/include/nxconfig.hxx b/NxWidgets/libnxwidgets/include/nxconfig.hxx index 67d359622c..cd51229c25 100644 --- a/NxWidgets/libnxwidgets/include/nxconfig.hxx +++ b/NxWidgets/libnxwidgets/include/nxconfig.hxx @@ -104,13 +104,13 @@ * Default dynamic array parameters. Default: 16, 8 * * CONFIG_NXWIDGETS_DEFAULT_BACKGROUNDCOLOR - Normal background color. Default: - * MKRGB(160,160,160) + * MKRGB(148,189,215) * CONFIG_NXWIDGETS_DEFAULT_SELECTEDBACKGROUNDCOLOR - Default selected background - * color. Default: MKRGB(120,192,192) + * color. Default: MKRGB(206,227,241) * CONFIG_NXWIDGETS_DEFAULT_SHINEEDGECOLOR - Shiny side boarder color. Default * MKRGB(248,248,248) * CONFIG_NXWIDGETS_DEFAULT_SHADOWEDGECOLOR - Shadowed side border color. - * Default: MKRGB(0,0,0) + * Default: MKRGB(35,58,73) * CONFIG_NXWIDGETS_DEFAULT_HIGHLIGHTCOLOR - Highlight color. Default: * MKRGB(192,192,192) * CONFIG_NXWIDGETS_DEFAULT_DISABLEDTEXTCOLOR - Text color on a disabled widget: @@ -362,7 +362,7 @@ */ #ifndef CONFIG_NXWIDGETS_DEFAULT_BACKGROUNDCOLOR -# define CONFIG_NXWIDGETS_DEFAULT_BACKGROUNDCOLOR MKRGB(160,160,160) +# define CONFIG_NXWIDGETS_DEFAULT_BACKGROUNDCOLOR MKRGB(148,189,215) #endif /** @@ -370,7 +370,7 @@ */ #ifndef CONFIG_NXWIDGETS_DEFAULT_SELECTEDBACKGROUNDCOLOR -# define CONFIG_NXWIDGETS_DEFAULT_SELECTEDBACKGROUNDCOLOR MKRGB(120,192,192) +# define CONFIG_NXWIDGETS_DEFAULT_SELECTEDBACKGROUNDCOLOR MKRGB(206,227,241) #endif /** @@ -386,7 +386,7 @@ */ #ifndef CONFIG_NXWIDGETS_DEFAULT_SHADOWEDGECOLOR -# define CONFIG_NXWIDGETS_DEFAULT_SHADOWEDGECOLOR MKRGB(0,0,0) +# define CONFIG_NXWIDGETS_DEFAULT_SHADOWEDGECOLOR MKRGB(35,58,73) #endif /** diff --git a/apps/ChangeLog.txt b/apps/ChangeLog.txt index 46ccccc7fb..d24494f54a 100644 --- a/apps/ChangeLog.txt +++ b/apps/ChangeLog.txt @@ -340,3 +340,8 @@ Kate. * apps/netutils/webserver/httpd.c: Add support for Keep-alive connections (from Kate). + * apps/NxWidget/Kconfig: This is a kludge. I created this NxWidgets + directory that ONLY contains Kconfig. NxWidgets does not like in + either the nuttx/ or the apps/ source trees. This kludge makes it + possible to configure NxWidgets/NxWM without too much trouble (with + the tradeoff being a kind ugly structure and some maintenance issues). diff --git a/apps/Kconfig b/apps/Kconfig index 1f38c58baf..5543ba72dd 100644 --- a/apps/Kconfig +++ b/apps/Kconfig @@ -27,6 +27,10 @@ menu "NSH Library" source "$APPSDIR/nshlib/Kconfig" endmenu +menu "NxWidgets/NxWM" +source "$APPSDIR/NxWidgets/Kconfig" +endmenu + menu "System NSH Add-Ons" source "$APPSDIR/system/Kconfig" endmenu diff --git a/apps/NxWidgets/Kconfig b/apps/NxWidgets/Kconfig new file mode 100644 index 0000000000..1d7852507c --- /dev/null +++ b/apps/NxWidgets/Kconfig @@ -0,0 +1,575 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# + +menuconfig NXWIDGETS + bool "Enable NxWidgets" + default n + depends on NX && HAVE_CXX + ---help--- + Enable support for NxWidgets + +if NXWIDGETS +comment "NX Server/Device Configuration" + +config NXWIDGETS_DEVNO + int "LCD Device Number" + default 0 + ---help--- + LCD device number (in case there are more than one LCDs connected). + Default: 0 + +config NXWIDGETS_VPLANE + int "Plane Number" + default 0 + ---help--- + Only a single video plane is supported. Default: 0 + +config NXWIDGETS_SERVERPRIO + int "NX Server priority" + default 51 + ---help--- + Priority of the NX server. This applies only if NX is configured in + multi-user mode (NX_MULTIUSER=y). Default: 51. + + NOTE: Of the three priority definitions here, NXWIDGETS_SERVERPRIO + should have the highest priority to avoid data overrun race conditions. + Such errors would most likely appear as duplicated rows of data on the + display. + +config NXWIDGETS_SERVERSTACK + int "NX Server Stack Size" + default 2048 + ---help--- + NX server thread stack size (in multi-user mode). Default 2048 + +config NXWIDGETS_CLIENTPRIO + int "NX Client Priority" + default 50 + ---help--- + The thread that calls CNxServer::connect() will be re-prioritized to + this priority. This applies only if NX is configured in multi-user + mode (NX_MULTIUSER=y). Default: 50 + +config NXWIDGETS_LISTENERPRIO + int "NX Listener Priority" + default 50 + ---help--- + Priority of the NX event listener thread. This applies only if NX + is configured in multi-user mode (NX_MULTIUSER=y). Default: 50 + +config NXWIDGETS_LISTENERSTACK + int "NX Listener Stack Size" + default 2048 + ---help--- + NX listener thread stack size (in multi-user mode). Default 2048 + +config NXWIDGETS_EXTERNINIT + bool "Extern LCD Initialization" + ---help--- + Define to support external display initialization. + +config NXWIDGET_EVENTWAIT + bool "Event Waiting" + default n + ---help--- + Build in support for external window event, modal loop management + logic. This includes methods to wait for windows events to occur + so that looping logic can sleep until something interesting happens + with the window. + +comment "NXWidget Configuration" + +config NXWIDGETS_BPP + int "BPP" + ---help--- + Supported bits-per-pixel {8, 16, 24, 32}. Default: The smallest + BPP configuration supported by NX. + +config NXWIDGETS_SIZEOFCHAR + int "Size of a character (1 or 2 bytes)" + range 1 2 + ---help--- + Size of character {1 or 2 bytes}. Default Determined by + NXWIDGETS_SIZEOFCHAR + +comment "NXWidget Default Values" + +config NXWIDGETS_DEFAULT_FONTID + int "Default Font ID" + ---help--- + Default font ID. Default: NXFONT_DEFAULT + +config NXWIDGETS_TNXARRAY_INITIALSIZE + int "Initial Size of Dynamic Arrays" + default 16 + ---help--- + Default dynamic array size (in entries). Default: 16 + +config NXWIDGETS_TNXARRAY_SIZEINCREMENT + int "Dyanamic Array Reallocation Size Increment" + default 8 + ---help--- + Default dynamic array realloctino increment (in entries). Default: 8 + +config NXWIDGETS_DEFAULT_BACKGROUNDCOLOR + hex "Normal Background Color" + ---help--- + Normal background color. Default: RGB(148,189,215) + +config NXWIDGETS_DEFAULT_SELECTEDBACKGROUNDCOLOR + hex "Selected Background Color" + ---help--- + Default selected background color. Default: RGB(206,227,241) + +config NXWIDGETS_DEFAULT_SHINEEDGECOLOR + hex "Shiny Edge Color" + ---help--- + Shiny side boarder color. Default: RGB(248,248,248) + +config NXWIDGETS_DEFAULT_SHADOWEDGECOLOR + hex "Shadow Edge Color" + ---help--- + Shadowed side border color. Default: RGB(35,58,73) + +config NXWIDGETS_DEFAULT_HIGHLIGHTCOLOR + hex "Highlight Color" + ---help--- + Highlight color. Default: RGB(192,192,192) + +config NXWIDGETS_DEFAULT_DISABLEDTEXTCOLOR + hex "Disabled Text Color" + ---help--- + Text color on a disabled widget: Default: RGB(192,192,192) + +config NXWIDGETS_DEFAULT_ENABLEDTEXTCOLOR + hex "Enabled Text Color" + ---help--- + Text color on a enabled widget. Default: RGB(248,248,248) + +config NXWIDGETS_DEFAULT_SELECTEDTEXTCOLOR + hex "Selected Text Color" + ---help--- + Text color on a selected widget. Default: RGB(0,0,0) + +config NXWIDGETS_DEFAULT_FONTCOLOR + hex "Default Font Color" + ---help--- + Default font color. Default: RGB(255,255,255) + +config NXWIDGETS_TRANSPARENT_COLOR + hex "Transparent Color" + ---help--- + Transparent color. Default: RGB(0,0,0) + +comment "Keypad behavior" + +config NXWIDGETS_FIRST_REPEAT_TIME + int "First Repeat Time" + default 500 + ---help--- + Time taken before a key starts repeating (in milliseconds). Default: 500 + +config NXWIDGETS_CONTINUE_REPEAT_TIME + int "Continue Repeat Time" + default 200 + ---help--- + Time taken before a repeating key repeats again (in milliseconds). + Default: 200 + +config NXWIDGETS_DOUBLECLICK_TIME + int "Double Click Time" + default 350 + ---help--- + Left button release-press time for double click (in milliseconds). + Default: 350 + +config NXWIDGETS_KBDBUFFER_SIZE + int "Keybard Buffer Size" + default 16 + ---help--- + Size of incoming character buffer, i.e., the maximum number of + characters that can be entered between NX polling cycles without + losing data. + +config NXWIDGETS_CURSORCONTROL_SIZE + int "Cursor Control Buffer Size" + default 4 + ---help--- + Size of incoming cursor control buffer, i.e., the maximum number + of cursor controls that can between entered by NX polling cycles + without losing data. Default: 4 + +endif + +menuconfig NXWM + bool "Enable NxWM" + default n + depends on NXWIDGETS && NX_MULTIUSER + ---help--- + Enable support for the NuttX Tiny Window Manager (NxWM) + +if NXWM +comment "General settings" + +config NXWM_DEFAULT_FONTID + int "Font ID" + ---help--- + The NxWM default font ID. Default: NXFONT_DEFAULT + +config NXWM_UNITTEST + bool "NxWM Unit Test" + default n + ---help--- + Enable Hooks for the NxWM Unit Test + +comment "Color configuration" + +config NXWM_DEFAULT_BACKGROUNDCOLOR + hex "Background Color" + ---help--- + Normal background color. Default: RGB(148,189,215) + +config NXWM_DEFAULT_SELECTEDBACKGROUNDCOLOR + hex "Normal Background Color" + ---help--- + Select background color. Default: RGB(206,227,241) + +config NXWM_DEFAULT_SHINEEDGECOLOR + hex "Shiny Edge Color" + ---help--- + Color of the bright edge of a border. Default: RGB(255,255,255) + +config NXWM_DEFAULT_SHADOWEDGECOLOR + hex "Shadow Edge Color" + ---help--- + Color of the shadowed edge of a border. Default: RGB(0,0,0) + +config NXWM_DEFAULT_FONTCOLOR + hex "Default Font Color" + ---help--- + Default fong color. Default: RGB(0,0,0) + +config NXWM_TRANSPARENT_COLOR + hex "Transparent Color" + ---help--- + The "transparent" color. Default: RGB(0,0,0) + +comment "Horizontal and vertical spacing of icons in the task bar" + +config NXWM_TASKBAR_VSPACING + int "Vertical Spacing" + default 2 + ---help--- + Vertical spacing. Default: 2 pixels + +config NXWM_TASKBAR_HSPACING + int "Horizontal Spacing" + default 2 + ---help--- + Horizontal spacing. Default: 2 rows + +choice NXWM_TASKBAR_LOCATION + prompt "Taskbar Location" + default NXWM_TASKBAR_TOP + +config NXWM_TASKBAR_TOP + bool "Top" + ---help--- + Task bar is at the top of the display + +config NXWM_TASKBAR_BOTTOM + bool "Bottom" + ---help--- + Task bar is at the bottom of the display + +config NXWM_TASKBAR_LEFT + bool "Left" + ---help--- + Task bar is on the left side of the display + +config NXWM_TASKBAR_RIGHT + bool "Right" + ---help--- + Task bar is on the right side of the display + +endchoice + +config NXWM_TASKBAR_WIDTH + int "Taskbar Width" + ---help--- + Task bar thickness (either vertical or horizontal). Default: 25 + 2*spacing + +comment "Tool Bar Configuration" + +config NXWM_TOOLBAR_HEIGHT + int "Toolbar Height" + ---help--- + The height of the tool bar in each application window. At present, + all icons are 21 pixels in height and, hence, require a task bar of + at least that size. + +comment "Background Image" + +config NXWM_BACKGROUND_IMAGE + string "Background Image" + ---help--- + The name of the image to use in the background window. Default: + NXWidgets::g_nuttxBitmap + +comment "Start Window Configuration" + +comment "Horizontal and vertical spacing of icons in the task bar" + +config NXWM_STARTWINDOW_VSPACING + int "Vertical Spacing" + default 4 + ---help--- + Vertical spacing. Default: 4 pixels + +config NXWM_STARTWINDOW_HSPACING + int "Horizontal Spacing" + default 4 + ---help--- + Horizontal spacing. Default: 4 rows + +config NXWM_STARTWINDOW_ICON + string "StartWindow Icon" + ---help--- + The glyph to use as the start window icon. Default: NxWM::g_playBitmap + +config NXWM_STARTWINDOW_MQNAME + string "Message Queue Name" + default "/dev/nxwm" + ---help--- + The well known name of the message queue. Used to communicated from + CWindowMessenger to the start window thread. Default: "/dev/nxwm" + +config NXWM_STARTWINDOW_MXMSGS + int "Max Messages" + default 32 + ---help--- + The maximum number of messages to queue before blocking. Defualt 32 + +config NXWM_STARTWINDOW_MXMPRIO + int "Message Priority" + default 42 + ---help--- + The message priority. Default: 42. + +config NXWM_STARTWINDOW_PRIO + int "StartWindow Task Priority" + default 50 + ---help--- + Priority of the StartWindow task. Default: 50. + + NOTE: This priority should be less than NXWIDGETS_SERVERPRIO or else + there may be data overrun errors. Such errors would most likely appear + as duplicated rows of data on the display. + +config NXWM_STARTWINDOW_STACKSIZE + int "StartWindow Task Stack Size" + default 2048 + ---help--- + The stack size to use when starting the StartWindow task. Default: + 2048 bytes. + +comment "NxConsole Window Configuration" + +config NXWM_NXCONSOLE_PRIO + int "NxConsole Task Priority" + default 50 + ---help--- + Priority of the NxConsole task. Default: 50. + + NOTE: This priority should be less than NXWIDGETS_SERVERPRIO or + else there may be data overrun errors. Such errors would most likely + appear as duplicated rows of data on the display. + +config NXWM_NXCONSOLE_STACKSIZE + int "NxConsole Task Stack Size" + default 2048 + ---help--- + The stack size to use when starting the NxConsole task. Default: + 2048 bytes. + +config NXWM_NXCONSOLE_WCOLOR + hex "NxConsole Background Color" + ---help--- + The color of the NxConsole window background. Default: + RGB(192,192,192) + +config NXWM_NXCONSOLE_FONTCOLOR + hex "NxConsole Font Color" + ---help--- + The color of the fonts to use in the NxConsole window. + Default: RGB(0,0,0) + +config NXWM_NXCONSOLE_FONTID + int "NxConsole Font ID" + ---help--- + The ID of the font to use in the NxConsole window. Default: + NXWM_DEFAULT_FONTID + +config NXWM_NXCONSOLE_ICON + string "NxConsole Icon" + ---help--- + The glyph to use as the NxConsole icon. Default: NxWM::g_cmdBitmap + +config NXWM_TOUCHSCREEN + bool "Touchscreen Support" + default y if INPUT + default n if !INPUT + ---help--- + Define to build in touchscreen support. + +if NXWM_TOUCHSCREEN +comment "Touchscreen device settings" + +config NXWM_TOUCHSCREEN_DEVNO + int "Touchscreen Device Number" + default 0 + ---help--- + Touchscreen device minor number, i.e., the N in /dev/inputN. + Default: 0 + +config NXWM_TOUCHSCREEN_DEVPATH + string "Touchscreen Device Path" + default "/dev/input0" + ---help--- + The full path to the touchscreen device. Default: "/dev/input0" + +config NXWM_TOUCHSCREEN_SIGNO + int "Touchscreen Signal Number" + default 5 + ---help--- + The realtime signal used to wake up the touchscreen listener + thread. Default: 5 + +config NXWM_TOUCHSCREEN_LISTENERPRIO + int "Touchscreen Listener Task Priority" + default 50 + ---help--- + Priority of the touchscreen listener thread. Default: 50 + +config NXWM_TOUCHSCREEN_LISTENERSTACK + int "Touchscreen Listener Task Stack Size" + ---help--- + Touchscreen listener thread stack size. Default 1024 + +endif + +config NXWM_KEYBOARD + bool "Keyboard Support" + default n + ---help--- + Define to build in touchscreen support. + +if NXWM_KEYBOARD +comment "Keyboard device settings" + +config NXWM_KEYBOARD_DEVPATH + string "Keyboard Device Path" + default "/dev/console" + ---help--- + The full path to the touchscreen device. Default: "/dev/console" + +config NXWM_KEYBOARD_SIGNO + int "Keyboard Task Signal Number" + default 6 + ---help--- + The realtime signal used to wake up the touchscreen listener thread. + Default: 6 + +config NXWM_KEYBOARD_BUFSIZE + int "Keyboard Buffer Size" + default 16 + ---help--- + The size of the keyboard read data buffer. Default: 16 + +config NXWM_KEYBOARD_LISTENERPRIO + int "Keyboard Listener Task Priority" + default 50 + ---help--- + Priority of the touchscreen listener thread. Default: 50 + +config NXWM_KEYBOARD_LISTENERSTACK + int "Keyboard Listener Task Stack Size" + default 2048 + ---help--- + Keyboard listener thread stack size. Default: 1024 + +endif + +comment "Calibration display settings" + +config NXWM_CALIBRATION_BACKGROUNDCOLOR + hex "Background Color" + ---help--- + The background color of the touchscreen calibration display. + Default: Same as NXWM_DEFAULT_BACKGROUNDCOLOR. + +config NXWM_CALIBRATION_LINECOLOR + hex "Line Color" + ---help--- + The color of the lines used in the touchscreen calibration display. + Default: RGB(0, 0, 128) (dark blue) + +config NXWM_CALIBRATION_CIRCLECOLOR + hex "Normal Circle Color" + ---help--- + The color of the circle in the touchscreen calibration display. + Default: RGB(255, 255, 255) (white) + +config NXWM_CALIBRATION_TOUCHEDCOLOR + hex "Touched Circle Color" + ---help--- + The color of the circle in the touchscreen calibration display after + the touch is recorder. Default: RGB(255, 255, 96) (very light yellow) + +config NXWM_CALIBRATION_ICON + string "Callibration Icon" + ---help--- + The ICON to use for the touchscreen calibration application. Default: + NxWM::g_calibrationBitmap + +config NXWM_CALIBRATION_SIGNO + int "Calibration Signal Number" + default 5 + ---help--- + The realtime signal used to wake up the touchscreen calibration + thread. Default: 5 + +config NXWM_CALIBRATION_LISTENERPRIO + int "Calibration Task Priority" + default 50 + ---help--- + Priority of the calibration listener thread. Default: 50 + +config NXWM_CALIBRATION_LISTENERSTACK + int "Calibration Task Stack Size" + default 2048 + ---help--- + Calibration listener thread stack size. Default 2048 + +comment "Calibration display settings" + +config NXWM_HEXCALCULATOR_BACKGROUNDCOLOR + hex "Calculator Background Color" + ---help--- + The background color of the calculator display. Default: Same + as NXWM_DEFAULT_BACKGROUNDCOLOR + +config NXWM_HEXCALCULATOR_ICON + string "Calculator Icon" + ---help--- + The ICON to use for the hex calculator application. Default: + NxWM::g_calculatorBitmap + +config NXWM_HEXCALCULATOR_FONTID + int "Calculator Font ID" + ---help--- + The font used with the calculator. Default: NXWM_DEFAULT_FONTID + +endif diff --git a/apps/NxWidgets/README.txt b/apps/NxWidgets/README.txt new file mode 100644 index 0000000000..835ea781e1 --- /dev/null +++ b/apps/NxWidgets/README.txt @@ -0,0 +1,11 @@ +apps/NxWidgets README File +========================== + +The NxWidgets and NxWM libraries don't physically reside in the apps/ source +tree. The source code actually resides at the top level NxWidgets/ +directory. This directory is just a kludge...it is here only support +configuration of NxWidgets and NxWM. + +The only files that reside in this directory are (1) this README.txt file +and (2) the Kconfig file to support NxWidgets configuration. This is a +duplicate of the NxWidgets file that you can file at NxWidgets/Kconfig. diff --git a/nuttx/ChangeLog b/nuttx/ChangeLog index 56929ad2f5..c64112ecbe 100644 --- a/nuttx/ChangeLog +++ b/nuttx/ChangeLog @@ -3400,3 +3400,10 @@ * configs/shenzhou/src/up_lcd.c: Oops. Shenzhou LCD does not have an SSD1289 controller. Its an ILI93xx. Ported the STM3240G-EVAL ILI93xx driver to work on the Shenzhou board. + * configs/shenzhou/nxwm: Added an NxWM configuratino for the + Shenzhou board. This is untested on initial check-in. It will + be used to verify the Shenzhou LCD driver (and eventually the + touchscreen driver). + * configs/shenzhou/src/up_touchscreen.c: Add ADS7843E touchscreen + support for the Shenzhou board. The initial check-in is untested + and basically a clone of the the touchscreen support fro the SAM-3U. diff --git a/nuttx/configs/shenzhou/nxwm/defconfig b/nuttx/configs/shenzhou/nxwm/defconfig new file mode 100644 index 0000000000..805d547391 --- /dev/null +++ b/nuttx/configs/shenzhou/nxwm/defconfig @@ -0,0 +1,1196 @@ +# +# Automatically generated file; DO NOT EDIT. +# Nuttx/ Configuration +# +CONFIG_NUTTX_NEWCONFIG=y + +# +# Build Setup +# +# CONFIG_EXPERIMENTAL is not set + +# +# Build Configuration +# +# CONFIG_APPS_DIR="../apps" +# CONFIG_BUILD_2PASS is not set + +# +# Binary Output Formats +# +# CONFIG_RRLOAD_BINARY is not set +CONFIG_INTELHEX_BINARY=y +# CONFIG_MOTOROLA_SREC is not set +# CONFIG_RAW_BINARY is not set + +# +# Customize Header Files +# +# CONFIG_ARCH_STDBOOL_H is not set +# CONFIG_ARCH_MATH_H is not set +# CONFIG_ARCH_STDARG_H is not set + +# +# Debug Options +# +# CONFIG_DEBUG is not set +# CONFIG_DEBUG_SYMBOLS is not set + +# +# System Type +# +# CONFIG_ARCH_8051 is not set +CONFIG_ARCH_ARM=y +# CONFIG_ARCH_AVR is not set +# CONFIG_ARCH_HC is not set +# CONFIG_ARCH_MIPS is not set +# CONFIG_ARCH_RGMP is not set +# CONFIG_ARCH_SH is not set +# CONFIG_ARCH_SIM is not set +# CONFIG_ARCH_X86 is not set +# CONFIG_ARCH_Z16 is not set +# CONFIG_ARCH_Z80 is not set +CONFIG_ARCH="arm" +# CONFIG_ARCH_CHIP_C5471 is not set +# CONFIG_ARCH_CHIP_CALYPSO is not set +# CONFIG_ARCH_CHIP_DM320 is not set +# CONFIG_ARCH_CHIP_IMX is not set +# CONFIG_ARCH_CHIP_KINETIS is not set +# CONFIG_ARCH_CHIP_LM3S is not set +# CONFIG_ARCH_CHIP_LPC17XX is not set +# CONFIG_ARCH_CHIP_LPC214X is not set +# CONFIG_ARCH_CHIP_LPC2378 is not set +# CONFIG_ARCH_CHIP_LPC31XX is not set +# CONFIG_ARCH_CHIP_LPC43XX is not set +# CONFIG_ARCH_CHIP_SAM3U is not set +CONFIG_ARCH_CHIP_STM32=y +# CONFIG_ARCH_CHIP_STR71X is not set +CONFIG_ARCH_CORTEXM3=y +CONFIG_ARCH_FAMILY="armv7-m" +CONFIG_ARCH_CHIP="stm32" +CONFIG_ARCH_HAVE_MPU=y +# CONFIG_ARMV7M_MPU is not set +CONFIG_ARCH_IRQPRIO=y +CONFIG_BOARD_LOOPSPERMSEC=10926 +# CONFIG_ARCH_CALIBRATION is not set +# CONFIG_SERIAL_TERMIOS is not set +# CONFIG_NET_MULTICAST is not set + +# +# STM32 Configuration Options +# +# CONFIG_ARCH_CHIP_STM32F100C8 is not set +# CONFIG_ARCH_CHIP_STM32F100CB is not set +# CONFIG_ARCH_CHIP_STM32F100R8 is not set +# CONFIG_ARCH_CHIP_STM32F100RB is not set +# CONFIG_ARCH_CHIP_STM32F100V8 is not set +# CONFIG_ARCH_CHIP_STM32F100VB is not set +# CONFIG_ARCH_CHIP_STM32F103RET6 is not set +# CONFIG_ARCH_CHIP_STM32F103VCT6 is not set +# CONFIG_ARCH_CHIP_STM32F103VET6 is not set +# CONFIG_ARCH_CHIP_STM32F103ZET6 is not set +# CONFIG_ARCH_CHIP_STM32F105VBT7 is not set +CONFIG_ARCH_CHIP_STM32F107VC=y +# CONFIG_ARCH_CHIP_STM32F207IG is not set +# CONFIG_ARCH_CHIP_STM32F405RG is not set +# CONFIG_ARCH_CHIP_STM32F405VG is not set +# CONFIG_ARCH_CHIP_STM32F405ZG is not set +# CONFIG_ARCH_CHIP_STM32F407VE is not set +# CONFIG_ARCH_CHIP_STM32F407VG is not set +# CONFIG_ARCH_CHIP_STM32F407ZE is not set +# CONFIG_ARCH_CHIP_STM32F407ZG is not set +# CONFIG_ARCH_CHIP_STM32F407IE is not set +# CONFIG_ARCH_CHIP_STM32F407IG is not set +CONFIG_STM32_STM32F10XX=y +CONFIG_STM32_CONNECTIVITYLINE=y +CONFIG_STM32_CODESOURCERYW=y +# CONFIG_STM32_CODESOURCERYL is not set +# CONFIG_STM32_ATOLLIC_LITE is not set +# CONFIG_STM32_ATOLLIC_PRO is not set +# CONFIG_STM32_DEVKITARM is not set +# CONFIG_STM32_RAISONANCE is not set +# CONFIG_STM32_BUILDROOT is not set +# CONFIG_STM32_DFU is not set + +# +# STM32 Peripheral Support +# +# CONFIG_STM32_ADC1 is not set +# CONFIG_STM32_ADC2 is not set +# CONFIG_STM32_ADC3 is not set +# CONFIG_STM32_CRC is not set +# CONFIG_STM32_DMA1 is not set +# CONFIG_STM32_DMA2 is not set +# CONFIG_STM32_BKP is not set +# CONFIG_STM32_CAN1 is not set +# CONFIG_STM32_DAC1 is not set +# CONFIG_STM32_DAC2 is not set +CONFIG_STM32_ETHMAC=y +CONFIG_STM32_I2C1=y +# CONFIG_STM32_I2C2 is not set +# CONFIG_STM32_IWDG is not set +CONFIG_STM32_PWR=y +# CONFIG_STM32_SPI1 is not set +# CONFIG_STM32_SPI2 is not set +# CONFIG_STM32_SPI4 is not set +# CONFIG_STM32_TIM1 is not set +# CONFIG_STM32_TIM2 is not set +# CONFIG_STM32_TIM3 is not set +# CONFIG_STM32_TIM4 is not set +# CONFIG_STM32_TIM5 is not set +# CONFIG_STM32_TIM6 is not set +# CONFIG_STM32_TIM7 is not set +# CONFIG_STM32_TIM8 is not set +# CONFIG_STM32_USART1 is not set +CONFIG_STM32_USART2=y +# CONFIG_STM32_USART3 is not set +# CONFIG_STM32_UART4 is not set +# CONFIG_STM32_UART5 is not set +# CONFIG_STM32_USB is not set +# CONFIG_STM32_WWDG is not set +CONFIG_STM32_I2C=y + +# +# Alternate Pin Mapping +# +# CONFIG_STM32_USART2_REMAP is not set +# CONFIG_STM32_I2C1_REMAP is not set +# CONFIG_STM32_ETH_REMAP is not set +# CONFIG_STM32_JTAG_DISABLE is not set +CONFIG_STM32_JTAG_FULL_ENABLE=y +# CONFIG_STM32_JTAG_NOJNTRST_ENABLE is not set +# CONFIG_STM32_JTAG_SW_ENABLE is not set +# CONFIG_STM32_FORCEPOWER is not set +# CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG is not set + +# +# I2C Configuration +# +# CONFIG_STM32_I2C_DYNTIMEO is not set +CONFIG_STM32_I2CTIMEOSEC=0 +CONFIG_STM32_I2CTIMEOMS=500 +CONFIG_STM32_I2CTIMEOTICKS=500 +# CONFIG_STM32_I2C_DUTY16_9 is not set + +# +# Ethernet MAC configuration +# +CONFIG_STM32_PHYADDR=0 +CONFIG_STM32_MII=y +CONFIG_STM32_MII_MCO=y +# CONFIG_STM32_MII_EXTCLK is not set +CONFIG_STM32_AUTONEG=y +CONFIG_STM32_PHYSR=16 +# CONFIG_STM32_PHYSR_ALTCONFIG is not set +CONFIG_STM32_PHYSR_SPEED=0x0002 +CONFIG_STM32_PHYSR_100MBPS=0x0000 +CONFIG_STM32_PHYSR_MODE=0x0004 +CONFIG_STM32_PHYSR_FULLDUPLEX=0x0004 +# CONFIG_STM32_ETH_PTP is not set + +# +# USB Host Configuration +# + +# +# Architecture Options +# +# CONFIG_ARCH_NOINTC is not set +# CONFIG_ARCH_DMA is not set +CONFIG_ARCH_STACKDUMP=y + +# +# Board Settings +# +CONFIG_DRAM_START=0x20000000 +CONFIG_DRAM_SIZE=196608 +CONFIG_ARCH_HAVE_INTERRUPTSTACK=y +CONFIG_ARCH_INTERRUPTSTACK=0 + +# +# Boot options +# +# CONFIG_BOOT_RUNFROMEXTSRAM is not set +CONFIG_BOOT_RUNFROMFLASH=y +# CONFIG_BOOT_RUNFROMISRAM is not set +# CONFIG_BOOT_RUNFROMSDRAM is not set +# CONFIG_BOOT_COPYTORAM is not set + +# +# Board Selection +# +# CONFIG_ARCH_BOARD_OLIMEX_STM32P107 is not set +CONFIG_ARCH_BOARD_SHENZHOU=y +# CONFIG_ARCH_BOARD_CUSTOM is not set +CONFIG_ARCH_BOARD="shenzhou" + +# +# Common Board Options +# +CONFIG_ARCH_HAVE_LEDS=y +CONFIG_ARCH_LEDS=y +CONFIG_ARCH_HAVE_BUTTONS=y +# CONFIG_ARCH_BUTTONS is not set +CONFIG_ARCH_HAVE_IRQBUTTONS=y +CONFIG_NSH_MMCSDMINOR=0 + +# +# Board-Specific Options +# + +# +# LCD Controller Selection +# + +# +# Disable Unused LCD Controllers +# +# CONFIG_STM32_ILI1505_DISABLE is not set +# CONFIG_STM32_ILI9300_DISABLE is not set +# CONFIG_STM32_ILI9320_DISABLE is not set +# CONFIG_STM32_ILI9321_DISABLE is not set +# CONFIG_STM32_ILI9325_DISABLE is not set +# CONFIG_STM32_ILI9328_DISABLE is not set +# CONFIG_STM32_ILI9331_DISABLE is not set +# CONFIG_STM32_ILI9919_DISABLE is not set + +# +# RTOS Features +# +CONFIG_MSEC_PER_TICK=10 +CONFIG_RR_INTERVAL=200 +# CONFIG_SCHED_INSTRUMENTATION is not set +CONFIG_TASK_NAME_SIZE=0 +# CONFIG_JULIAN_TIME is not set +CONFIG_START_YEAR=2012 +CONFIG_START_MONTH=5 +CONFIG_START_DAY=28 +CONFIG_DEV_CONSOLE=y +# CONFIG_DEV_LOWCONSOLE is not set +# CONFIG_MUTEX_TYPES is not set +# CONFIG_PRIORITY_INHERITANCE is not set +# CONFIG_FDCLONE_DISABLE is not set +# CONFIG_FDCLONE_STDIO is not set +CONFIG_SDCLONE_DISABLE=y +CONFIG_SCHED_WORKQUEUE=y +CONFIG_SCHED_WORKPRIORITY=192 +CONFIG_SCHED_WORKPERIOD=50000 +CONFIG_SCHED_WORKSTACKSIZE=2048 +CONFIG_SIG_SIGWORK=4 +# CONFIG_SCHED_LPWORK is not set +CONFIG_SCHED_WAITPID=y +# CONFIG_SCHED_ATEXIT is not set +CONFIG_SCHED_ONEXIT=y +CONFIG_SCHED_ONEXIT_MAX=1 +CONFIG_USER_ENTRYPOINT="nxwm_main" +CONFIG_DISABLE_OS_API=y +# CONFIG_DISABLE_CLOCK is not set +# CONFIG_DISABLE_POSIX_TIMERS is not set +# CONFIG_DISABLE_PTHREAD is not set +# CONFIG_DISABLE_SIGNALS is not set +# CONFIG_DISABLE_MQUEUE is not set +# CONFIG_DISABLE_MOUNTPOINT is not set +# CONFIG_DISABLE_ENVIRON is not set +CONFIG_DISABLE_POLL=y + +# +# Sizes of configurable things (0 disables) +# +CONFIG_MAX_TASKS=16 +CONFIG_MAX_TASK_ARGS=4 +CONFIG_NPTHREAD_KEYS=4 +CONFIG_NFILE_DESCRIPTORS=12 +CONFIG_NFILE_STREAMS=12 +CONFIG_NAME_MAX=32 +CONFIG_PREALLOC_MQ_MSGS=32 +CONFIG_MQ_MAXMSGSIZE=48 +CONFIG_MAX_WDOGPARMS=2 +CONFIG_PREALLOC_WDOGS=8 +CONFIG_PREALLOC_TIMERS=4 + +# +# Stack and heap information +# +# CONFIG_CUSTOM_STACK is not set +CONFIG_IDLETHREAD_STACKSIZE=1024 +CONFIG_USERMAIN_STACKSIZE=2048 +CONFIG_PTHREAD_STACK_MIN=256 +CONFIG_PTHREAD_STACK_DEFAULT=2048 + +# +# Device Drivers +# +CONFIG_DEV_NULL=y +# CONFIG_DEV_ZERO is not set +# CONFIG_LOOP is not set +# CONFIG_RAMDISK is not set +# CONFIG_CAN is not set +# CONFIG_PWM is not set +CONFIG_I2C=y +# CONFIG_I2C_SLAVE is not set +CONFIG_I2C_TRANSFER=y +# CONFIG_I2C_WRITEREAD is not set +CONFIG_I2C_POLLED=y +# CONFIG_I2C_TRACE is not set +CONFIG_ARCH_HAVE_I2CRESET=y +# CONFIG_I2C_RESET is not set +CONFIG_SPI=y +# CONFIG_SPI_OWNBUS is not set +CONFIG_SPI_EXCHANGE=y +# CONFIG_SPI_CMDDATA is not set +CONFIG_RTC=y +CONFIG_RTC_DATETIME=y +# CONFIG_RTC_ALARM is not set +# CONFIG_WATCHDOG is not set +# CONFIG_ANALOG is not set +# CONFIG_BCH is not set +CONFIG_INPUT=y +# CONFIG_INPUT_TSC2007 is not set +CONFIG_INPUT_ADS7843E=y +CONFIG_LCD=y +# CONFIG_LCD_NOGETRUN is not set +CONFIG_LCD_MAXCONTRAST=1 +CONFIG_LCD_MAXPOWER=1 +# CONFIG_LCD_P14201 is not set +# CONFIG_LCD_NOKIA6100 is not set +# CONFIG_LCD_UG9664HSWAG01 is not set +CONFIG_LCD_LANDSCAPE=y +# CONFIG_LCD_PORTRAIT is not set +# CONFIG_LCD_RPORTRAIT is not set +# CONFIG_LCD_RLANDSCAPE is not set +# CONFIG_MMCSD is not set +# CONFIG_MTD is not set +# CONFIG_NETDEVICES is not set +# CONFIG_NET_SLIP is not set +# CONFIG_PIPES is not set +# CONFIG_PM is not set +# CONFIG_POWER is not set +# CONFIG_SENSORS is not set +# CONFIG_SERCOMM_CONSOLE is not set +CONFIG_SERIAL=y +# CONFIG_LOWLEVEL_CONSOLE is not set +# CONFIG_16550_UART is not set +CONFIG_ARCH_HAVE_USART2=y +CONFIG_MCU_SERIAL=y +CONFIG_STANDARD_SERIAL=y +CONFIG_USART2_SERIAL_CONSOLE=y +# CONFIG_NO_SERIAL_CONSOLE is not set + +# +# USART2 Configuration +# +CONFIG_USART2_RXBUFSIZE=128 +CONFIG_USART2_TXBUFSIZE=128 +CONFIG_USART2_BAUD=115200 +CONFIG_USART2_BITS=8 +CONFIG_USART2_PARITY=0 +CONFIG_USART2_2STOP=0 +# CONFIG_USBDEV is not set +# CONFIG_USBHOST is not set +# CONFIG_WIRELESS is not set + +# +# System Logging Device Options +# + +# +# System Logging +# +# CONFIG_RAMLOG is not set + +# +# Networking Support +# +CONFIG_NET=y +# CONFIG_NET_NOINTS is not set +CONFIG_NET_MULTIBUFFER=y +# CONFIG_NET_IPv6 is not set +CONFIG_NSOCKET_DESCRIPTORS=10 +CONFIG_NET_NACTIVESOCKETS=16 +CONFIG_NET_SOCKOPTS=y +CONFIG_NET_BUFSIZE=562 +# CONFIG_NET_TCPURGDATA is not set +CONFIG_NET_TCP=y +CONFIG_NET_TCP_CONNS=40 +CONFIG_NET_MAX_LISTENPORTS=40 +CONFIG_NET_TCP_READAHEAD_BUFSIZE=562 +CONFIG_NET_NTCP_READAHEAD_BUFFERS=16 +CONFIG_NET_TCP_RECVDELAY=0 +CONFIG_NET_TCPBACKLOG=y +CONFIG_NET_UDP=y +CONFIG_NET_UDP_CHECKSUMS=y +CONFIG_NET_UDP_CONNS=8 +# CONFIG_NET_BROADCAST is not set +CONFIG_NET_ICMP=y +CONFIG_NET_ICMP_PING=y +# CONFIG_NET_PINGADDRCONF is not set +# CONFIG_NET_IGMP is not set +CONFIG_NET_STATISTICS=y +CONFIG_NET_RECEIVE_WINDOW=562 +CONFIG_NET_ARPTAB_SIZE=16 +# CONFIG_NET_ARP_IPIN is not set + +# +# File Systems +# + +# +# File system configuration +# +CONFIG_FS_FAT=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FAT_MAXFNAME=32 +# CONFIG_FS_FATTIME is not set +# CONFIG_FAT_DMAMEMORY is not set +# CONFIG_FS_RAMMAP is not set +# CONFIG_NFS is not set +# CONFIG_FS_NXFFS is not set +# CONFIG_FS_ROMFS is not set + +# +# System Logging +# +# CONFIG_SYSLOG is not set + +# +# Graphics Support +# +CONFIG_NX=y +CONFIG_NX_LCDDRIVER=y +CONFIG_NX_NPLANES=1 +# CONFIG_NX_WRITEONLY is not set + +# +# Supported Pixel Depths +# +CONFIG_NX_DISABLE_1BPP=y +CONFIG_NX_DISABLE_2BPP=y +CONFIG_NX_DISABLE_4BPP=y +CONFIG_NX_DISABLE_8BPP=y +# CONFIG_NX_DISABLE_16BPP is not set +CONFIG_NX_DISABLE_24BPP=y +CONFIG_NX_DISABLE_32BPP=y +# CONFIG_NX_PACKEDMSFIRST is not set + +# +# Input Devices +# +CONFIG_NX_MOUSE=y +CONFIG_NX_KBD=y + +# +# Framed Window Borders +# +CONFIG_NXTK_BORDERWIDTH=4 +CONFIG_NXTK_BORDERCOLOR1=0x5cb7 +CONFIG_NXTK_BORDERCOLOR2=0x21c9 +CONFIG_NXTK_BORDERCOLOR3=0xffdf +# CONFIG_NXTK_AUTORAISE is not set + +# +# Font Selections +# +CONFIG_NXFONTS_CHARBITS=7 +# CONFIG_NXFONT_SANS17X22 is not set +# CONFIG_NXFONT_SANS20X26 is not set +CONFIG_NXFONT_SANS23X27=y +# CONFIG_NXFONT_SANS22X29 is not set +# CONFIG_NXFONT_SANS28X37 is not set +# CONFIG_NXFONT_SANS39X48 is not set +# CONFIG_NXFONT_SANS17X23B is not set +# CONFIG_NXFONT_SANS20X27B is not set +CONFIG_NXFONT_SANS22X29B=y +# CONFIG_NXFONT_SANS28X37B is not set +# CONFIG_NXFONT_SANS40X49B is not set +# CONFIG_NXFONT_SERIF22X29 is not set +# CONFIG_NXFONT_SERIF29X37 is not set +# CONFIG_NXFONT_SERIF38X48 is not set +# CONFIG_NXFONT_SERIF22X28B is not set +# CONFIG_NXFONT_SERIF27X38B is not set +# CONFIG_NXFONT_SERIF38X49B is not set +CONFIG_NXCONSOLE=y + +# +# NxConsole Output Text/Graphics Options +# +CONFIG_NXCONSOLE_BPP=16 +CONFIG_NXCONSOLE_CURSORCHAR=137 +CONFIG_NXCONSOLE_MXCHARS=325 +CONFIG_NXCONSOLE_CACHESIZE=32 +CONFIG_NXCONSOLE_LINESEPARATION=0 +# CONFIG_NXCONSOLE_NOWRAP is not set + +# +# NxConsole Input options +# +CONFIG_NXCONSOLE_NXKBDIN=y +CONFIG_NXCONSOLE_KBDBUFSIZE=16 +CONFIG_NXCONSOLE_NPOLLWAITERS=4 + +# +# NX Multi-user only options +# +CONFIG_NX_MULTIUSER=y +CONFIG_NX_BLOCKING=y +CONFIG_NX_MXSERVERMSGS=32 +CONFIG_NX_MXCLIENTMSGS=16 + +# +# Memory Management +# +# CONFIG_MM_SMALL is not set +CONFIG_MM_REGIONS=1 +# CONFIG_GRAN is not set + +# +# Library Routines +# +CONFIG_STDIO_BUFFER_SIZE=64 +CONFIG_STDIO_LINEBUFFER=y +CONFIG_NUNGET_CHARS=2 +CONFIG_LIB_HOMEDIR="/" +# CONFIG_HAVE_LIBM is not set +# CONFIG_NOPRINTF_FIELDWIDTH is not set +# CONFIG_LIBC_FLOATINGPOINT is not set +# CONFIG_EOL_IS_CR is not set +# CONFIG_EOL_IS_LF is not set +# CONFIG_EOL_IS_BOTH_CRLF is not set +CONFIG_EOL_IS_EITHER_CRLF=y +# CONFIG_LIBC_STRERROR is not set +# CONFIG_LIBC_PERROR_STDOUT is not set +CONFIG_ARCH_LOWPUTC=y +CONFIG_LIB_SENDFILE_BUFSIZE=512 +# CONFIG_ARCH_ROMGETC is not set +# CONFIG_ARCH_OPTIMIZED_FUNCTIONS is not set +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +# CONFIG_CXX_NEWLONG is not set + +# +# Application Configuration +# + +# +# Named Applications +# +CONFIG_NAMEDAPP=y + +# +# Examples +# + +# +# ADC Example +# +# CONFIG_EXAMPLES_ADC is not set + +# +# Buttons Example +# +# CONFIG_EXAMPLES_BUTTONS is not set + +# +# CAN Example +# +# CONFIG_EXAMPLES_CAN is not set + +# +# USB CDC/ACM Class Driver Example +# +# CONFIG_EXAMPLES_CDCACM is not set + +# +# USB composite Class Driver Example +# +# CONFIG_EXAMPLES_COMPOSITE is not set + +# +# DHCP Server Example +# +# CONFIG_EXAMPLES_DHCPD is not set + +# +# FTP Client Example +# +# CONFIG_EXAMPLES_FTPC is not set + +# +# FTP Server Example +# +# CONFIG_EXAMPLES_FTPD is not set + +# +# "Hello, World!" Example +# +# CONFIG_EXAMPLES_HELLO is not set + +# +# "Hello, World!" C++ Example +# +# CONFIG_EXAMPLES_HELLOXX is not set + +# +# USB HID Keyboard Example +# +# CONFIG_EXAMPLES_HIDKBD is not set + +# +# IGMP Example +# +# CONFIG_EXAMPLES_IGMP is not set + +# +# LCD Read/Write Example +# +# CONFIG_EXAMPLES_LCDRW is not set + +# +# Memory Management Example +# +# CONFIG_EXAMPLES_MM is not set + +# +# File System Mount Example +# +# CONFIG_EXAMPLES_MOUNT is not set + +# +# FreeModBus Example +# +# CONFIG_EXAMPLES_MODBUS is not set + +# +# Network Test Example +# +# CONFIG_EXAMPLES_NETTEST is not set + +# +# NuttShell (NSH) Example +# +# CONFIG_EXAMPLES_NSH is not set + +# +# NULL Example +# +# CONFIG_EXAMPLES_NULL is not set + +# +# NX Graphics Example +# +# CONFIG_EXAMPLES_NX is not set + +# +# NxConsole Example +# +# CONFIG_EXAMPLES_NXCONSOLE is not set + +# +# NXFFS File System Example +# +# CONFIG_EXAMPLES_NXFFS is not set + +# +# NXFLAT Example +# +# CONFIG_EXAMPLES_NXFLAT is not set + +# +# NX Graphics "Hello, World!" Example +# +# CONFIG_EXAMPLES_NXHELLO is not set + +# +# NX Graphics image Example +# +# CONFIG_EXAMPLES_NXIMAGE is not set + +# +# NX Graphics lines Example +# +# CONFIG_EXAMPLES_NXLINES is not set + +# +# NX Graphics Text Example +# +# CONFIG_EXAMPLES_NXTEXT is not set + +# +# OS Test Example +# +# CONFIG_EXAMPLES_OSTEST is not set + +# +# Pascal "Hello, World!"example +# +# CONFIG_EXAMPLES_PASHELLO is not set + +# +# Pipe Example +# +# CONFIG_EXAMPLES_PIPE is not set + +# +# Poll Example +# +# CONFIG_EXAMPLES_POLL is not set + +# +# Pulse Width Modulation (PWM) Example +# + +# +# Quadrature Encoder Example +# +# CONFIG_EXAMPLES_QENCODER is not set + +# +# RGMP Example +# +# CONFIG_EXAMPLES_RGMP is not set + +# +# ROMFS Example +# +# CONFIG_EXAMPLES_ROMFS is not set + +# +# sendmail Example +# +# CONFIG_EXAMPLES_SENDMAIL is not set + +# +# Serial Loopback Example +# +# CONFIG_EXAMPLES_SERLOOP is not set + +# +# Telnet Daemon Example +# +# CONFIG_EXAMPLES_TELNETD is not set + +# +# THTTPD Web Server Example +# +# CONFIG_EXAMPLES_THTTPD is not set + +# +# TIFF Generation Example +# +# CONFIG_EXAMPLES_TIFF is not set + +# +# Touchscreen Example +# +# CONFIG_EXAMPLES_TOUCHSCREEN is not set + +# +# UDP Example +# +# CONFIG_EXAMPLES_UDP is not set + +# +# UDP Discovery Daemon Example +# +# CONFIG_EXAMPLE_DISCOVER is not set + +# +# uIP Web Server Example +# +# CONFIG_EXAMPLES_UIP is not set + +# +# USB Serial Test Example +# +# CONFIG_EXAMPLES_USBSERIAL is not set + +# +# USB Mass Storage Class Example +# +# CONFIG_EXAMPLES_USBMSC is not set + +# +# USB Serial Terminal Example +# +# CONFIG_EXAMPLES_USBTERM is not set + +# +# Watchdog timer Example +# +# CONFIG_EXAMPLES_WATCHDOG is not set + +# +# wget Example +# +# CONFIG_EXAMPLES_WGET is not set + +# +# WLAN Example +# +# CONFIG_EXAMPLES_WLAN is not set + +# +# XML RPC Example +# + +# +# Interpreters +# + +# +# Interpreters +# +# CONFIG_FICL is not set +# CONFIG_PCODE is not set + +# +# Network Utilities +# + +# +# Networking Utilities +# + +# +# DHCP client +# +# CONFIG_NETUTILS_DHCPC is not set + +# +# DHCP server +# +# CONFIG_NETUTILS_DHCPD is not set + +# +# FTP client +# +# CONFIG_NETUTILS_FTPC is not set + +# +# FTP server +# +# CONFIG_NETUTILS_FTPD is not set + +# +# Name resolution +# +CONFIG_NETUTILS_RESOLV=y +CONFIG_NET_RESOLV_ENTRIES=4 + +# +# SMTP +# +# CONFIG_NETUTILS_SMTP is not set + +# +# TFTP client +# +CONFIG_NETUTILS_TELNETD=y + +# +# TFTP client +# +CONFIG_NETUTILS_TFTPC=y + +# +# THTTPD web server +# +# CONFIG_NETUTILS_THTTPD is not set + +# +# uIP support library +# +CONFIG_NETUTILS_UIPLIB=y + +# +# uIP web client +# +CONFIG_NETUTILS_WEBCLIENT=y + +# +# uIP web server +# +# CONFIG_NETUTILS_WEBSERVER is not set + +# +# UDP Discovery Utility +# +# CONFIG_NETUTILS_DISCOVER is not set + +# +# XML-RPC library +# +# CONFIG_NETUTILS_XMLRPC is not set + +# +# ModBus +# + +# +# FreeModbus +# +# CONFIG_MODBUS is not set + +# +# NSH Library +# +CONFIG_NSH_LIBRARY=y +CONFIG_NSH_BUILTIN_APPS=y + +# +# Disable Individual commands +# +# CONFIG_NSH_DISABLE_CAT is not set +# CONFIG_NSH_DISABLE_CD is not set +# CONFIG_NSH_DISABLE_CP is not set +# CONFIG_NSH_DISABLE_DD is not set +# CONFIG_NSH_DISABLE_ECHO is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_FREE is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_HELP is not set +# CONFIG_NSH_DISABLE_IFCONFIG is not set +# CONFIG_NSH_DISABLE_KILL is not set +# CONFIG_NSH_DISABLE_LOSETUP is not set +# CONFIG_NSH_DISABLE_LS is not set +# CONFIG_NSH_DISABLE_MB is not set +# CONFIG_NSH_DISABLE_MKDIR is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_MKFIFO is not set +# CONFIG_NSH_DISABLE_MKRD is not set +# CONFIG_NSH_DISABLE_MH is not set +# CONFIG_NSH_DISABLE_MOUNT is not set +# CONFIG_NSH_DISABLE_MW is not set +# CONFIG_NSH_DISABLE_NSFMOUNT is not set +# CONFIG_NSH_DISABLE_PS is not set +# CONFIG_NSH_DISABLE_PING is not set +# CONFIG_NSH_DISABLE_PUT is not set +# CONFIG_NSH_DISABLE_PWD is not set +# CONFIG_NSH_DISABLE_RM is not set +# CONFIG_NSH_DISABLE_RMDIR is not set +# CONFIG_NSH_DISABLE_SET is not set +# CONFIG_NSH_DISABLE_SH is not set +# CONFIG_NSH_DISABLE_SLEEP is not set +# CONFIG_NSH_DISABLE_TEST is not set +# CONFIG_NSH_DISABLE_UMOUNT is not set +# CONFIG_NSH_DISABLE_UNSET is not set +# CONFIG_NSH_DISABLE_USLEEP is not set +# CONFIG_NSH_DISABLE_WGET is not set +# CONFIG_NSH_DISABLE_XD is not set +CONFIG_NSH_FILEIOSIZE=512 +CONFIG_NSH_LINELEN=64 +CONFIG_NSH_NESTDEPTH=3 +# CONFIG_NSH_DISABLESCRIPT is not set +# CONFIG_NSH_DISABLEBG is not set +CONFIG_NSH_CONSOLE=y +# CONFIG_NSH_CONDEV is not set +# CONFIG_NSH_ARCHINIT is not set +CONFIG_NSH_TELNET=y +CONFIG_NSH_TELNETD_PORT=23 +CONFIG_NSH_TELNETD_DAEMONPRIO=100 +CONFIG_NSH_TELNETD_DAEMONSTACKSIZE=2048 +CONFIG_NSH_TELNETD_CLIENTPRIO=100 +CONFIG_NSH_TELNETD_CLIENTSTACKSIZE=2048 +CONFIG_NSH_IOBUFFER_SIZE=512 +CONFIG_NSH_IPADDR=0x0a000002 +CONFIG_NSH_DRIPADDR=0x0a000001 +CONFIG_NSH_NETMASK=0xffffff00 +CONFIG_NSH_NOMAC=y + +# +# NxWidgets/NxWM +# +CONFIG_NXWIDGETS=y + +# +# NX Server/Device Configuration +# +CONFIG_NXWIDGETS_DEVNO=0 +CONFIG_NXWIDGETS_VPLANE=0 +CONFIG_NXWIDGETS_SERVERPRIO=51 +CONFIG_NXWIDGETS_SERVERSTACK=2048 +CONFIG_NXWIDGETS_CLIENTPRIO=50 +CONFIG_NXWIDGETS_LISTENERPRIO=50 +CONFIG_NXWIDGETS_LISTENERSTACK=2048 +# CONFIG_NXWIDGETS_EXTERNINIT is not set +# CONFIG_NXWIDGET_EVENTWAIT is not set + +# +# NXWidget Configuration +# +CONFIG_NXWIDGETS_BPP=16 +CONFIG_NXWIDGETS_SIZEOFCHAR=1 + +# +# NXWidget Default Values +# +CONFIG_NXWIDGETS_DEFAULT_FONTID= +CONFIG_NXWIDGETS_TNXARRAY_INITIALSIZE=16 +CONFIG_NXWIDGETS_TNXARRAY_SIZEINCREMENT=8 +CONFIG_NXWIDGETS_DEFAULT_BACKGROUNDCOLOR= +CONFIG_NXWIDGETS_DEFAULT_SELECTEDBACKGROUNDCOLOR= +CONFIG_NXWIDGETS_DEFAULT_SHINEEDGECOLOR= +CONFIG_NXWIDGETS_DEFAULT_SHADOWEDGECOLOR= +CONFIG_NXWIDGETS_DEFAULT_HIGHLIGHTCOLOR= +CONFIG_NXWIDGETS_DEFAULT_DISABLEDTEXTCOLOR= +CONFIG_NXWIDGETS_DEFAULT_ENABLEDTEXTCOLOR= +CONFIG_NXWIDGETS_DEFAULT_SELECTEDTEXTCOLOR= +CONFIG_NXWIDGETS_DEFAULT_FONTCOLOR= +CONFIG_NXWIDGETS_TRANSPARENT_COLOR= + +# +# Keypad behavior +# +CONFIG_NXWIDGETS_FIRST_REPEAT_TIME=500 +CONFIG_NXWIDGETS_CONTINUE_REPEAT_TIME=200 +CONFIG_NXWIDGETS_DOUBLECLICK_TIME=350 +CONFIG_NXWIDGETS_KBDBUFFER_SIZE=16 +CONFIG_NXWIDGETS_CURSORCONTROL_SIZE=4 +CONFIG_NXWM=y + +# +# General settings +# +CONFIG_NXWM_DEFAULT_FONTID= +CONFIG_NXWM_UNITTEST=y + +# +# Color configuration +# +CONFIG_NXWM_DEFAULT_BACKGROUNDCOLOR= +CONFIG_NXWM_DEFAULT_SELECTEDBACKGROUNDCOLOR= +CONFIG_NXWM_DEFAULT_SHINEEDGECOLOR= +CONFIG_NXWM_DEFAULT_SHADOWEDGECOLOR= +CONFIG_NXWM_DEFAULT_FONTCOLOR= +CONFIG_NXWM_TRANSPARENT_COLOR= + +# +# Horizontal and vertical spacing of icons in the task bar +# +CONFIG_NXWM_TASKBAR_VSPACING=4 +CONFIG_NXWM_TASKBAR_HSPACING=2 +# CONFIG_NXWM_TASKBAR_TOP is not set +# CONFIG_NXWM_TASKBAR_BOTTOM is not set +CONFIG_NXWM_TASKBAR_LEFT=y +# CONFIG_NXWM_TASKBAR_RIGHT is not set +CONFIG_NXWM_TASKBAR_WIDTH= + +# +# Tool Bar Configuration +# +CONFIG_NXWM_TOOLBAR_HEIGHT= + +# +# Background Image +# +CONFIG_NXWM_BACKGROUND_IMAGE="" + +# +# Start Window Configuration +# + +# +# Horizontal and vertical spacing of icons in the task bar +# +CONFIG_NXWM_STARTWINDOW_VSPACING=4 +CONFIG_NXWM_STARTWINDOW_HSPACING=4 +CONFIG_NXWM_STARTWINDOW_ICON="" +CONFIG_NXWM_STARTWINDOW_MQNAME="/dev/nxwm" +CONFIG_NXWM_STARTWINDOW_MXMSGS=32 +CONFIG_NXWM_STARTWINDOW_MXMPRIO=42 +CONFIG_NXWM_STARTWINDOW_PRIO=50 +CONFIG_NXWM_STARTWINDOW_STACKSIZE=2048 + +# +# NxConsole Window Configuration +# +CONFIG_NXWM_NXCONSOLE_PRIO=50 +CONFIG_NXWM_NXCONSOLE_STACKSIZE=2048 +CONFIG_NXWM_NXCONSOLE_WCOLOR= +CONFIG_NXWM_NXCONSOLE_FONTCOLOR= +CONFIG_NXWM_NXCONSOLE_FONTID= +CONFIG_NXWM_NXCONSOLE_ICON="" +CONFIG_NXWM_TOUCHSCREEN=y + +# +# Touchscreen device settings +# +CONFIG_NXWM_TOUCHSCREEN_DEVNO=0 +CONFIG_NXWM_TOUCHSCREEN_DEVPATH="/dev/input0" +CONFIG_NXWM_TOUCHSCREEN_SIGNO=5 +CONFIG_NXWM_TOUCHSCREEN_LISTENERPRIO=50 +CONFIG_NXWM_TOUCHSCREEN_LISTENERSTACK= +CONFIG_NXWM_KEYBOARD=y + +# +# Keyboard device settings +# +CONFIG_NXWM_KEYBOARD_DEVPATH="/dev/console" +CONFIG_NXWM_KEYBOARD_SIGNO=6 +CONFIG_NXWM_KEYBOARD_BUFSIZE=16 +CONFIG_NXWM_KEYBOARD_LISTENERPRIO=50 +CONFIG_NXWM_KEYBOARD_LISTENERSTACK=2048 + +# +# Calibration display settings +# +CONFIG_NXWM_CALIBRATION_BACKGROUNDCOLOR= +CONFIG_NXWM_CALIBRATION_LINECOLOR= +CONFIG_NXWM_CALIBRATION_CIRCLECOLOR= +CONFIG_NXWM_CALIBRATION_TOUCHEDCOLOR= +CONFIG_NXWM_CALIBRATION_ICON="" +CONFIG_NXWM_CALIBRATION_SIGNO=5 +CONFIG_NXWM_CALIBRATION_LISTENERPRIO=50 +CONFIG_NXWM_CALIBRATION_LISTENERSTACK=2048 + +# +# Calibration display settings +# +CONFIG_NXWM_HEXCALCULATOR_BACKGROUNDCOLOR= +CONFIG_NXWM_HEXCALCULATOR_ICON="" +CONFIG_NXWM_HEXCALCULATOR_FONTID=5 + +# +# System NSH Add-Ons +# + +# +# Custom free memory command +# +# CONFIG_SYSTEM_FREE is not set + +# +# I2C tool +# +CONFIG_SYSTEM_I2CTOOL=y +CONFIG_I2CTOOL_BUILTIN=y +CONFIG_I2CTOOL_MINBUS=1 +CONFIG_I2CTOOL_MAXBUS=3 +CONFIG_I2CTOOL_MINADDR=0x03 +CONFIG_I2CTOOL_MAXADDR=0x77 +CONFIG_I2CTOOL_MAXREGADDR=0xff +CONFIG_I2CTOOL_DEFFREQ=100000 + +# +# FLASH Program Installation +# +# CONFIG_SYSTEM_INSTALL is not set + +# +# readline() support +# +CONFIG_SYSTEM_READLINE=y +CONFIG_READLINE_ECHO=y + +# +# VSN board Add-Ons +# + +# +# VSN board add-ons +# +# CONFIG_VSN_POWEROFF is not set +# CONFIG_VSN_RAMTRON is not set +# CONFIG_VSN_SDCARD is not set +# CONFIG_VSN_SYSINFO is not set diff --git a/nuttx/configs/shenzhou/src/Makefile b/nuttx/configs/shenzhou/src/Makefile index fc1d6caa1f..04088110c8 100644 --- a/nuttx/configs/shenzhou/src/Makefile +++ b/nuttx/configs/shenzhou/src/Makefile @@ -92,6 +92,10 @@ else CSRCS += up_lcd.c endif +ifeq ($(CONFIG_INPUT_ADS7843E),y) +CSRCS += up_touchscreen.c +endif + COBJS = $(CSRCS:.c=$(OBJEXT)) SRCS = $(ASRCS) $(CSRCS) diff --git a/nuttx/configs/shenzhou/src/up_touchscreen.c b/nuttx/configs/shenzhou/src/up_touchscreen.c new file mode 100644 index 0000000000..4ed34c0ec4 --- /dev/null +++ b/nuttx/configs/shenzhou/src/up_touchscreen.c @@ -0,0 +1,292 @@ +/************************************************************************************ + * configs/shenzhou/src/up_touchscreen.c + * arch/arm/src/board/up_touchscreen.c + * + * Copyright (C) 2011-2012 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 "stm32_internal.h" +#include "shenzhou_internal.h" + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ +/* Configuration ************************************************************/ + +#ifdef CONFIG_INPUT_ADS7843E +#ifndef CONFIG_INPUT +# error "Touchscreen support requires CONFIG_INPUT" +#endif + +#ifndef CONFIG_STM32_SPI3 +# error "Touchscreen support requires CONFIG_STM32_SPI3" +#endif + +#ifndef CONFIG_ADS7843E_FREQUENCY +# define CONFIG_ADS7843E_FREQUENCY 500000 +#endif + +#ifndef CONFIG_ADS7843E_SPIDEV +# define CONFIG_ADS7843E_SPIDEV 3 +#endif + +#if CONFIG_ADS7843E_SPIDEV != 3 +# error "CONFIG_ADS7843E_SPIDEV must be three" +#endif + +#ifndef CONFIG_ADS7843E_DEVMINOR +# define CONFIG_ADS7843E_DEVMINOR 0 +#endif + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +struct stm32_config_s +{ + struct ads7843e_config_s dev; + xcpt_t handler; +}; + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/* IRQ/GPIO access callbacks. These operations all hidden behind + * callbacks to isolate the ADS7843E driver from differences in GPIO + * interrupt handling by varying boards and MCUs. If possible, + * interrupts should be configured on both rising and falling edges + * so that contact and loss-of-contact events can be detected. + * + * attach - Attach the ADS7843E interrupt handler to the GPIO interrupt + * enable - Enable or disable the GPIO interrupt + * clear - Acknowledge/clear any pending GPIO interrupt + * pendown - Return the state of the pen down GPIO input + */ + +static int tsc_attach(FAR struct ads7843e_config_s *state, xcpt_t isr); +static void tsc_enable(FAR struct ads7843e_config_s *state, bool enable); +static void tsc_clear(FAR struct ads7843e_config_s *state); +static bool tsc_busy(FAR struct ads7843e_config_s *state); +static bool tsc_pendown(FAR struct ads7843e_config_s *state); + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/* A reference to a structure of this type must be passed to the ADS7843E + * driver. This structure provides information about the configuration + * of the ADS7843E and provides some board-specific hooks. + * + * Memory for this structure is provided by the caller. It is not copied + * by the driver and is presumed to persist while the driver is active. The + * memory must be writable because, under certain circumstances, the driver + * may modify frequency or X plate resistance values. + */ + +static struct stm32_config_s g_tscinfo = +{ + { + .frequency = CONFIG_ADS7843E_FREQUENCY, + .attach = tsc_attach, + .enable = tsc_enable, + .clear = tsc_clear, + .busy = tsc_busy, + .pendown = tsc_pendown, + }, + .handler = NULL, +}; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/* IRQ/GPIO access callbacks. These operations all hidden behind + * callbacks to isolate the ADS7843E driver from differences in GPIO + * interrupt handling by varying boards and MCUs. If possible, + * interrupts should be configured on both rising and falling edges + * so that contact and loss-of-contact events can be detected. + * + * attach - Attach the ADS7843E interrupt handler to the GPIO interrupt + * enable - Enable or disable the GPIO interrupt + * clear - Acknowledge/clear any pending GPIO interrupt + * pendown - Return the state of the pen down GPIO input + */ + +static int tsc_attach(FAR struct ads7843e_config_s *state, xcpt_t isr) +{ + FAR struct stm32_config_s *priv = (FAR struct stm32_config_s *)state; + + /* Just save the handler for use when the interrupt is enabled */ + + priv->handler = handler; +} + +static void tsc_enable(FAR struct ads7843e_config_s *state, bool enable) +{ + FAR struct stm32_config_s *priv = (FAR struct stm32_config_s *)state; + + DEBUGASSERT(priv->handler); + + /* Attach and enable, or detach and disable */ + + ivdbg("IRQ:%d enable:%d\n", STM32_TCS_IRQ, enable); + if (enable) + { + (void)stm32_gpiosetevent(GPIO_TP_INT, false, true, true, + priv->handler); + } + else + { + (void)stm32_gpiosetevent(GPIO_TP_INT, false, true, true, NULL); + } +} + +static void tsc_clear(FAR struct ads7843e_config_s *state) +{ + /* Does nothing */ +} + +static bool tsc_busy(FAR struct ads7843e_config_s *state) +{ + /* Hmmm... The ADS7843E BUSY pin is not brought out on the Shenzhou board. + * We will most certainly have to revisit this. + */ + + return false; +} + +static bool tsc_pendown(FAR struct ads7843e_config_s *state) +{ + /* REVISIT: This might need to be inverted */ + + bool pendown = stm32_gpioread(GPIO_TP_INT); + ivdbg("pendown:%d\n", pendown); + return pendown; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: arch_tcinitialize + * + * Description: + * Each board that supports a touchscreen device must provide this function. + * This function is called by application-specific, setup logic to + * configure the touchscreen device. This function will register the driver + * as /dev/inputN where N is the minor device number. + * + * Input Parameters: + * minor - The input device minor number + * + * Returned Value: + * Zero is returned on success. Otherwise, a negated errno value is + * returned to indicate the nature of the failure. + * + ****************************************************************************/ + +int arch_tcinitialize(int minor) +{ + FAR struct spi_dev_s *dev; + int ret; + + idbg("minor %d\n", minor); + DEBUGASSERT(minor == 0); + + /* Configure and enable the ADS7843E interrupt pin as an input. */ + + (void)stm32_configgpio(GPIO_TP_INT); + + /* Get an instance of the SPI interface */ + + dev = up_spiinitialize(CONFIG_ADS7843E_SPIDEV); + if (!dev) + { + idbg("Failed to initialize SPI bus %d\n", CONFIG_ADS7843E_SPIDEV); + return -ENODEV; + } + + /* Initialize and register the SPI touschscreen device */ + + ret = ads7843e_register(dev, &g_tscinfo, CONFIG_ADS7843E_DEVMINOR); + if (ret < 0) + { + idbg("Failed to initialize SPI bus %d\n", CONFIG_ADS7843E_SPIDEV); + /* up_spiuninitialize(dev); */ + return -ENODEV; + } + + return OK; +} + +/**************************************************************************** + * Name: arch_tcuninitialize + * + * Description: + * Each board that supports a touchscreen device must provide this function. + * This function is called by application-specific, setup logic to + * uninitialize the touchscreen device. + * + * Input Parameters: + * None + * + * Returned Value: + * None. + * + ****************************************************************************/ + +void arch_tcuninitialize(void) +{ + /* No support for un-initializing the touchscreen ADS7843E device yet */ +} + +#endif /* CONFIG_INPUT_ADS7843E */ + From 731b466aca72e22039f46678e890a50dd59b59b4 Mon Sep 17 00:00:00 2001 From: patacongo Date: Tue, 25 Sep 2012 22:47:11 +0000 Subject: [PATCH 40/56] If server fails to create a thread because of lack-of-resources (EAGAIN), don't terminate. Keep serving... Memory may become available again later. From Kate. git-svn-id: http://svn.code.sf.net/p/nuttx/code/trunk@5191 42af7a65-404d-4744-a932-0658087f49c3 --- apps/netutils/uiplib/uip_server.c | 35 ++++++++++++++++++++++++++----- 1 file changed, 30 insertions(+), 5 deletions(-) diff --git a/apps/netutils/uiplib/uip_server.c b/apps/netutils/uiplib/uip_server.c index d7d1d2c2ae..16be6f42fb 100644 --- a/apps/netutils/uiplib/uip_server.c +++ b/apps/netutils/uiplib/uip_server.c @@ -89,6 +89,7 @@ void uip_server(uint16_t portno, pthread_startroutine_t handler, int stacksize) socklen_t addrlen; int listensd; int acceptsd; + int ret; /* Create a new TCP socket to use to listen for connections */ @@ -102,22 +103,28 @@ void uip_server(uint16_t portno, pthread_startroutine_t handler, int stacksize) for (;;) { + /* Accept the next connectin */ + addrlen = sizeof(struct sockaddr_in); acceptsd = accept(listensd, (struct sockaddr*)&myaddr, &addrlen); if (acceptsd < 0) { ndbg("accept failure: %d\n", errno); - break;; + break; } nvdbg("Connection accepted -- spawning sd=%d\n", acceptsd); - /* Configure to "linger" until all data is sent when the socket is closed */ + /* Configure to "linger" until all data is sent when the socket is + * closed. + */ #ifdef CONFIG_NET_HAVE_SOLINGER ling.l_onoff = 1; ling.l_linger = 30; /* timeout is seconds */ - if (setsockopt(acceptsd, SOL_SOCKET, SO_LINGER, &ling, sizeof(struct linger)) < 0) + + ret = setsockopt(acceptsd, SOL_SOCKET, SO_LINGER, &ling, sizeof(struct linger)); + if (ret < 0) { close(acceptsd); ndbg("setsockopt SO_LINGER failure: %d\n", errno); @@ -132,10 +139,26 @@ void uip_server(uint16_t portno, pthread_startroutine_t handler, int stacksize) (void)pthread_attr_init(&attr); (void)pthread_attr_setstacksize(&attr, stacksize); - if (pthread_create(&child, &attr, handler, (void*)acceptsd) != 0) + ret = pthread_create(&child, &attr, handler, (void*)acceptsd); + if (ret != 0) { + /* Close the connection */ + close(acceptsd); - ndbg("create_create failed\n"); + ndbg("pthread_create failed\n"); + + if (ret == EAGAIN) + { + /* Lacked resources to create a new thread. This is a temporary + * condition, so we close this peer, but keep serving for + * other connections. + */ + + continue; + } + + /* Something is very wrong... Break out and stop serving */ + break; } @@ -146,5 +169,7 @@ void uip_server(uint16_t portno, pthread_startroutine_t handler, int stacksize) (void)pthread_detach(child); } + /* Close the listerner socket */ + close(listensd); } From 415e8303727189dff9e702edf61b22be027b292a Mon Sep 17 00:00:00 2001 From: patacongo Date: Tue, 25 Sep 2012 23:36:27 +0000 Subject: [PATCH 41/56] Fix README.html links -- again git-svn-id: http://svn.code.sf.net/p/nuttx/code/trunk@5192 42af7a65-404d-4744-a932-0658087f49c3 --- nuttx/Documentation/README.html | 320 ++++++++++++++++---------------- nuttx/README.txt | 2 + 2 files changed, 163 insertions(+), 159 deletions(-) diff --git a/nuttx/Documentation/README.html b/nuttx/Documentation/README.html index 105b936562..6d41a6f37a 100644 --- a/nuttx/Documentation/README.html +++ b/nuttx/Documentation/README.html @@ -22,256 +22,258 @@ diff --git a/nuttx/README.txt b/nuttx/README.txt index 5c330aa683..7477e0a245 100644 --- a/nuttx/README.txt +++ b/nuttx/README.txt @@ -848,6 +848,8 @@ apps | `- README.txt |- nshlib/ | `- README.txt + |- NxWidgets/ + | `- README.txt |- system/ | |- i2c/README.txt | |- free/README.txt From 201fdbc42c46bc9146a8cbf2434a98792d6d9f50 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 26 Sep 2012 10:11:57 +0200 Subject: [PATCH 42/56] ardrone flying now (still workaround of disabled rates controller) --- .../multirotor_att_control_main.c | 15 ++++++++------- apps/sensors/sensors.cpp | 4 +++- 2 files changed, 11 insertions(+), 8 deletions(-) diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 70b9d8e28d..904872dde2 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -127,8 +127,8 @@ static void *rate_control_thread_main(void *arg) gyro_lp[1] = gyro_report.y; gyro_lp[2] = gyro_report.z; -// multirotor_control_rates(&rates_sp, gyro_lp, &actuators); -// orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + multirotor_control_rates(&rates_sp, gyro_lp, &actuators); + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); // } } } @@ -192,10 +192,10 @@ mc_thread_main(int argc, char *argv[]) /* ready, spawn pthread */ pthread_attr_t rate_control_attr; - pthread_attr_init(&rate_control_attr); - pthread_attr_setstacksize(&rate_control_attr, 2048); - pthread_t rate_control_thread; - pthread_create(&rate_control_thread, &rate_control_attr, rate_control_thread_main, NULL); +// pthread_attr_init(&rate_control_attr); +// pthread_attr_setstacksize(&rate_control_attr, 2048); +// pthread_t rate_control_thread; +// pthread_create(&rate_control_thread, &rate_control_attr, rate_control_thread_main, NULL); while (!thread_should_exit) { @@ -306,7 +306,7 @@ mc_thread_main(int argc, char *argv[]) perf_print_counter(mc_loop_perf); perf_free(mc_loop_perf); - pthread_join(rate_control_thread, NULL); + //pthread_join(rate_control_thread, NULL); fflush(stdout); exit(0); @@ -340,6 +340,7 @@ int multirotor_att_control_main(int argc, char *argv[]) default: fprintf(stderr, "option: -%c\n", ch); usage("unrecognized option"); + break; } } argc -= optioncount; diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index eb22ac8a70..ceb8c4b104 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -968,7 +968,7 @@ Sensors::ppm_poll() _rc.chan[i].scaled = (ppm_buffer[i] - _parameters.trim[i]) / (float)(_parameters.max[i] - _parameters.trim[i]); } else if (ppm_buffer[i] < (_parameters.trim[i] - _parameters.dz[i])) { /* division by zero impossible for trim == min (as for throttle), as this falls in the above if clause */ - _rc.chan[i].scaled = -1.0f + ((ppm_buffer[i] - _parameters.min[i]) / (float)(_parameters.trim[i] - _parameters.min[i])); + _rc.chan[i].scaled = -((_parameters.trim[i] - ppm_buffer[i]) / (float)(_parameters.trim[i] - _parameters.min[i])); } else { /* in the configured dead zone, output zero */ @@ -1029,6 +1029,8 @@ Sensors::ppm_poll() orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc); orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control); + +// printf("SENSORS: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",manual_control.roll, manual_control.pitch, manual_control.yaw, manual_control.throttle); } #endif From ac43a67a0ff8be62504e3398def6b1f6f0719e14 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 26 Sep 2012 14:29:47 +0200 Subject: [PATCH 43/56] ardrone max motor output was slightly to high --- apps/ardrone_interface/ardrone_motor_control.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/ardrone_interface/ardrone_motor_control.c b/apps/ardrone_interface/ardrone_motor_control.c index cbf9600a59..89ed183ccd 100644 --- a/apps/ardrone_interface/ardrone_motor_control.c +++ b/apps/ardrone_interface/ardrone_motor_control.c @@ -372,7 +372,7 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls const float min_thrust = 0.02f; /**< 2% minimum thrust */ const float max_thrust = 1.0f; /**< 100% max thrust */ - const float scaling = 512.0f; /**< 100% thrust equals a value of 512 */ + const float scaling = 500.0f; /**< 100% thrust equals a value of 500 which works, 512 leads to cutoff */ const float min_gas = min_thrust * scaling; /**< value range sent to motors, minimum */ const float max_gas = max_thrust * scaling; /**< value range sent to motors, maximum */ From 923da1b34bd4b7fff1619735d80d343c9d4a204d Mon Sep 17 00:00:00 2001 From: patacongo Date: Wed, 26 Sep 2012 14:36:28 +0000 Subject: [PATCH 44/56] Fixes for clean compilation of NxWidgets/NxWM with Kconfig and changes to build system; Fixes to Shenzhou NxWM configuration for clean build git-svn-id: http://svn.code.sf.net/p/nuttx/code/trunk@5193 42af7a65-404d-4744-a932-0658087f49c3 --- NxWidgets/ChangeLog.txt | 2 + NxWidgets/libnxwidgets/Makefile | 8 ++++ NxWidgets/nxwm/Makefile | 8 ++++ nuttx/ChangeLog | 6 +++ nuttx/Documentation/README.html | 9 ++-- nuttx/Makefile | 3 +- nuttx/arch/arm/src/stm32/Kconfig | 9 +--- nuttx/configs/shenzhou/nxwm/defconfig | 48 ++++++++++++-------- nuttx/configs/shenzhou/scripts/ld.script | 8 +++- nuttx/configs/shenzhou/scripts/ld.script.dfu | 8 +++- nuttx/configs/shenzhou/src/up_touchscreen.c | 7 +-- nuttx/tools/cfgparser.c | 19 ++++++-- 12 files changed, 94 insertions(+), 41 deletions(-) diff --git a/NxWidgets/ChangeLog.txt b/NxWidgets/ChangeLog.txt index e0a4c9b2e1..840c0640bc 100644 --- a/NxWidgets/ChangeLog.txt +++ b/NxWidgets/ChangeLog.txt @@ -166,3 +166,5 @@ * Kconfig: Added a mconfig configuration file. Eventually, NxWidgets needs to get hooked into the NuttX mconf configuration. Still not exactly sure how to do that. +* libnxwidgets/Makefile and NxWidgets/nxwm/Makefile: Need updates + for consistency with recent changes to NuttX build system (>= 6.22) diff --git a/NxWidgets/libnxwidgets/Makefile b/NxWidgets/libnxwidgets/Makefile index 2520e419d5..0afb55e695 100644 --- a/NxWidgets/libnxwidgets/Makefile +++ b/NxWidgets/libnxwidgets/Makefile @@ -36,6 +36,14 @@ -include $(TOPDIR)/.config -include $(TOPDIR)/Make.defs +# Control build verbosity + +ifeq ($(V),1) +export Q := +else +export Q := @ +endif + NXWIDGETDIR := ${shell pwd | sed -e 's/ /\\ /g'} ASRCS = diff --git a/NxWidgets/nxwm/Makefile b/NxWidgets/nxwm/Makefile index e508634947..6b60327af5 100644 --- a/NxWidgets/nxwm/Makefile +++ b/NxWidgets/nxwm/Makefile @@ -36,6 +36,14 @@ -include $(TOPDIR)/.config -include $(TOPDIR)/Make.defs +# Control build verbosity + +ifeq ($(V),1) +export Q := +else +export Q := @ +endif + NXWMDIR := ${shell pwd | sed -e 's/ /\\ /g'} NXWIDGETDIR := $(NXWMDIR)/../libnxwidgets diff --git a/nuttx/ChangeLog b/nuttx/ChangeLog index c64112ecbe..3c4f142d12 100644 --- a/nuttx/ChangeLog +++ b/nuttx/ChangeLog @@ -3407,3 +3407,9 @@ * configs/shenzhou/src/up_touchscreen.c: Add ADS7843E touchscreen support for the Shenzhou board. The initial check-in is untested and basically a clone of the the touchscreen support fro the SAM-3U. + * tools/cfgparser.c: There are some NxWidget configuration + settings that must be de-quoted. + * arch/arm/src/stm32/Kconfig: There is no SPI4. Some platforms + SPI3 and some do not (still not clear). + * nuttx/configs/shenzhou: Various fixes to build new NxWM + configuration. diff --git a/nuttx/Documentation/README.html b/nuttx/Documentation/README.html index 6d41a6f37a..95102a004f 100644 --- a/nuttx/Documentation/README.html +++ b/nuttx/Documentation/README.html @@ -8,7 +8,7 @@

    NuttX README Files

    -

    Last Updated: September 12, 2012

    +

    Last Updated: September 25, 2012

    @@ -73,8 +73,8 @@ | | | |- src/README.txt | | | `- README.txt | | |- ekk-lm3s9b96/ - | | | `- README.txt - | | |- | | |- ez80f910200kitg/ + | | | `- README.txt + | | |- ez80f910200kitg/ | | | |- ostest/README.txt | | | `- README.txt | | |- ez80f910200zco/ @@ -93,7 +93,8 @@ | | | |- src/README.txt | | | `- README.txt | | |- lincoln60/ - | | | `- README.txt | | |- kwikstik-k40/ + | | | `- README.txt + | | |- kwikstik-k40/ | | | `- README.txt | | |- lm3s6432-s2e/ | | | |- include/README.txt diff --git a/nuttx/Makefile b/nuttx/Makefile index 3c6e9c1985..7a058d88e9 100644 --- a/nuttx/Makefile +++ b/nuttx/Makefile @@ -38,7 +38,8 @@ TOPDIR := ${shell pwd | sed -e 's/ /\\ /g'} -include ${TOPDIR}/tools/Config.mk -include ${TOPDIR}/Make.defs -# Control build verbosity. +# Control build verbosity + ifeq ($(V),1) export Q := else diff --git a/nuttx/arch/arm/src/stm32/Kconfig b/nuttx/arch/arm/src/stm32/Kconfig index 26c4b20c91..8d93fb104c 100644 --- a/nuttx/arch/arm/src/stm32/Kconfig +++ b/nuttx/arch/arm/src/stm32/Kconfig @@ -340,14 +340,7 @@ config STM32_SPI2 config STM32_SPI3 bool "SPI3" default n - depends on STM32_STM32F20XX || STM32_STM32F40XX - select SPI - select STM32_SPI - -config STM32_SPI4 - bool "SPI4" - default n - depends on STM32_STM32F10XX + depends on STM32_CONNECTIVITYLINE || STM32_STM32F20XX || STM32_STM32F40XX select SPI select STM32_SPI diff --git a/nuttx/configs/shenzhou/nxwm/defconfig b/nuttx/configs/shenzhou/nxwm/defconfig index 805d547391..9889e887de 100644 --- a/nuttx/configs/shenzhou/nxwm/defconfig +++ b/nuttx/configs/shenzhou/nxwm/defconfig @@ -71,7 +71,7 @@ CONFIG_ARCH_CHIP="stm32" CONFIG_ARCH_HAVE_MPU=y # CONFIG_ARMV7M_MPU is not set CONFIG_ARCH_IRQPRIO=y -CONFIG_BOARD_LOOPSPERMSEC=10926 +CONFIG_BOARD_LOOPSPERMSEC=5483 # CONFIG_ARCH_CALIBRATION is not set # CONFIG_SERIAL_TERMIOS is not set # CONFIG_NET_MULTICAST is not set @@ -121,7 +121,7 @@ CONFIG_STM32_CODESOURCERYW=y # CONFIG_STM32_CRC is not set # CONFIG_STM32_DMA1 is not set # CONFIG_STM32_DMA2 is not set -# CONFIG_STM32_BKP is not set +CONFIG_STM32_BKP=y # CONFIG_STM32_CAN1 is not set # CONFIG_STM32_DAC1 is not set # CONFIG_STM32_DAC2 is not set @@ -132,7 +132,7 @@ CONFIG_STM32_I2C1=y CONFIG_STM32_PWR=y # CONFIG_STM32_SPI1 is not set # CONFIG_STM32_SPI2 is not set -# CONFIG_STM32_SPI4 is not set +CONFIG_STM32_SPI3=y # CONFIG_STM32_TIM1 is not set # CONFIG_STM32_TIM2 is not set # CONFIG_STM32_TIM3 is not set @@ -148,14 +148,16 @@ CONFIG_STM32_USART2=y # CONFIG_STM32_UART5 is not set # CONFIG_STM32_USB is not set # CONFIG_STM32_WWDG is not set +CONFIG_STM32_SPI=y CONFIG_STM32_I2C=y # # Alternate Pin Mapping # -# CONFIG_STM32_USART2_REMAP is not set +CONFIG_STM32_USART2_REMAP=y +CONFIG_STM32_SPI3_REMAP=y # CONFIG_STM32_I2C1_REMAP is not set -# CONFIG_STM32_ETH_REMAP is not set +CONFIG_STM32_ETH_REMAP=y # CONFIG_STM32_JTAG_DISABLE is not set CONFIG_STM32_JTAG_FULL_ENABLE=y # CONFIG_STM32_JTAG_NOJNTRST_ENABLE is not set @@ -163,6 +165,12 @@ CONFIG_STM32_JTAG_FULL_ENABLE=y # CONFIG_STM32_FORCEPOWER is not set # CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG is not set +# +# SPI Configuration +# +# CONFIG_STM32_SPI_INTERRUPTS is not set +# CONFIG_STM32_SPI_DMA is not set + # # I2C Configuration # @@ -176,17 +184,19 @@ CONFIG_STM32_I2CTIMEOTICKS=500 # Ethernet MAC configuration # CONFIG_STM32_PHYADDR=0 -CONFIG_STM32_MII=y -CONFIG_STM32_MII_MCO=y -# CONFIG_STM32_MII_EXTCLK is not set +# CONFIG_STM32_MII is not set CONFIG_STM32_AUTONEG=y -CONFIG_STM32_PHYSR=16 -# CONFIG_STM32_PHYSR_ALTCONFIG is not set -CONFIG_STM32_PHYSR_SPEED=0x0002 -CONFIG_STM32_PHYSR_100MBPS=0x0000 -CONFIG_STM32_PHYSR_MODE=0x0004 -CONFIG_STM32_PHYSR_FULLDUPLEX=0x0004 +CONFIG_STM32_PHYSR=17 +CONFIG_STM32_PHYSR_ALTCONFIG=y +CONFIG_STM32_PHYSR_ALTMODE=0xf000 +CONFIG_STM32_PHYSR_10HD=0x1000 +CONFIG_STM32_PHYSR_100HD=0x4000 +CONFIG_STM32_PHYSR_10FD=0x2000 +CONFIG_STM32_PHYSR_100FD=0x8000 # CONFIG_STM32_ETH_PTP is not set +CONFIG_STM32_RMII=y +CONFIG_STM32_RMII_MCO=y +# CONFIG_STM32_RMII_EXTCLK is not set # # USB Host Configuration @@ -203,7 +213,7 @@ CONFIG_ARCH_STACKDUMP=y # Board Settings # CONFIG_DRAM_START=0x20000000 -CONFIG_DRAM_SIZE=196608 +CONFIG_DRAM_SIZE=65536 CONFIG_ARCH_HAVE_INTERRUPTSTACK=y CONFIG_ARCH_INTERRUPTSTACK=0 @@ -263,8 +273,8 @@ CONFIG_RR_INTERVAL=200 CONFIG_TASK_NAME_SIZE=0 # CONFIG_JULIAN_TIME is not set CONFIG_START_YEAR=2012 -CONFIG_START_MONTH=5 -CONFIG_START_DAY=28 +CONFIG_START_MONTH=9 +CONFIG_START_DAY=26 CONFIG_DEV_CONSOLE=y # CONFIG_DEV_LOWCONSOLE is not set # CONFIG_MUTEX_TYPES is not set @@ -339,7 +349,7 @@ CONFIG_SPI=y CONFIG_SPI_EXCHANGE=y # CONFIG_SPI_CMDDATA is not set CONFIG_RTC=y -CONFIG_RTC_DATETIME=y +# CONFIG_RTC_DATETIME is not set # CONFIG_RTC_ALARM is not set # CONFIG_WATCHDOG is not set # CONFIG_ANALOG is not set @@ -985,7 +995,7 @@ CONFIG_NSH_NESTDEPTH=3 # CONFIG_NSH_DISABLEBG is not set CONFIG_NSH_CONSOLE=y # CONFIG_NSH_CONDEV is not set -# CONFIG_NSH_ARCHINIT is not set +CONFIG_NSH_ARCHINIT=y CONFIG_NSH_TELNET=y CONFIG_NSH_TELNETD_PORT=23 CONFIG_NSH_TELNETD_DAEMONPRIO=100 diff --git a/nuttx/configs/shenzhou/scripts/ld.script b/nuttx/configs/shenzhou/scripts/ld.script index f07d4432d1..c9140291c0 100644 --- a/nuttx/configs/shenzhou/scripts/ld.script +++ b/nuttx/configs/shenzhou/scripts/ld.script @@ -63,10 +63,16 @@ SECTIONS _etext = ABSOLUTE(.); } > flash + .init_section : { + _sinit = ABSOLUTE(.); + *(.init_array .init_array.*) + _einit = ABSOLUTE(.); + } > flash + __exidx_start = ABSOLUTE(.); .ARM.exidx : { *(.ARM.exidx*) - } >flash + } > flash __exidx_end = ABSOLUTE(.); _eronly = ABSOLUTE(.); diff --git a/nuttx/configs/shenzhou/scripts/ld.script.dfu b/nuttx/configs/shenzhou/scripts/ld.script.dfu index b9d94c7a0f..c6d0d76b8e 100644 --- a/nuttx/configs/shenzhou/scripts/ld.script.dfu +++ b/nuttx/configs/shenzhou/scripts/ld.script.dfu @@ -65,10 +65,16 @@ SECTIONS _etext = ABSOLUTE(.); } > flash + .init_section : { + _sinit = ABSOLUTE(.); + *(.init_array .init_array.*) + _einit = ABSOLUTE(.); + } > flash + __exidx_start = ABSOLUTE(.); .ARM.exidx : { *(.ARM.exidx*) - } >flash + } > flash __exidx_end = ABSOLUTE(.); _eronly = ABSOLUTE(.); diff --git a/nuttx/configs/shenzhou/src/up_touchscreen.c b/nuttx/configs/shenzhou/src/up_touchscreen.c index 4ed34c0ec4..92b057f5d4 100644 --- a/nuttx/configs/shenzhou/src/up_touchscreen.c +++ b/nuttx/configs/shenzhou/src/up_touchscreen.c @@ -52,7 +52,7 @@ #include #include "stm32_internal.h" -#include "shenzhou_internal.h" +#include "shenzhou-internal.h" /**************************************************************************** * Pre-Processor Definitions @@ -159,13 +159,14 @@ static struct stm32_config_s g_tscinfo = * pendown - Return the state of the pen down GPIO input */ -static int tsc_attach(FAR struct ads7843e_config_s *state, xcpt_t isr) +static int tsc_attach(FAR struct ads7843e_config_s *state, xcpt_t handler) { FAR struct stm32_config_s *priv = (FAR struct stm32_config_s *)state; /* Just save the handler for use when the interrupt is enabled */ priv->handler = handler; + return OK; } static void tsc_enable(FAR struct ads7843e_config_s *state, bool enable) @@ -256,7 +257,7 @@ int arch_tcinitialize(int minor) /* Initialize and register the SPI touschscreen device */ - ret = ads7843e_register(dev, &g_tscinfo, CONFIG_ADS7843E_DEVMINOR); + ret = ads7843e_register(dev, &g_tscinfo.dev, CONFIG_ADS7843E_DEVMINOR); if (ret < 0) { idbg("Failed to initialize SPI bus %d\n", CONFIG_ADS7843E_SPIDEV); diff --git a/nuttx/tools/cfgparser.c b/nuttx/tools/cfgparser.c index e49b29d5e4..b1f189f6fb 100644 --- a/nuttx/tools/cfgparser.c +++ b/nuttx/tools/cfgparser.c @@ -56,13 +56,24 @@ char line[LINESIZE+1]; ****************************************************************************/ /* These are configuration variable name that are quoted by configuration tool - * but which must be unquoated when used in C code. + * but which must be unquoted when used in C code. */ static const char *dequote_list[] = { - "CONFIG_USER_ENTRYPOINT", - NULL + /* NuttX */ + + "CONFIG_USER_ENTRYPOINT", /* Name of entry point function */ + + /* NxWidgets/NxWM */ + + "CONFIG_NXWM_BACKGROUND_IMAGE", /* Name of bitmap image class */ + "CONFIG_NXWM_STARTWINDOW_ICON", /* Name of bitmap image class */ + "CONFIG_NXWM_NXCONSOLE_ICON", /* Name of bitmap image class */ + "CONFIG_NXWM_CALIBRATION_ICON", /* Name of bitmap image class */ + "CONFIG_NXWM_HEXCALCULATOR_ICON", /* Name of bitmap image class */ + + NULL /* Marks the end of the list */ }; /**************************************************************************** @@ -239,7 +250,7 @@ static char *dequote_value(const char *varname, char *varval) /* Handle the case where nothing is left after dequoting */ - if (len < 0) + if (len <= 0) { dqval = NULL; } From cbb1f1c9eda7f4eb3b4b8be8be80eeb1ad7b9209 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 26 Sep 2012 21:30:03 +0200 Subject: [PATCH 45/56] Fixed RC and offboard control state machine --- apps/commander/commander.c | 57 ++++++++++++++++++------------- apps/uORB/topics/vehicle_status.h | 1 + 2 files changed, 35 insertions(+), 23 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 4be004ad62..e653ef72f8 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1135,6 +1135,8 @@ int commander_thread_main(int argc, char *argv[]) memset(¤t_status, 0, sizeof(current_status)); current_status.state_machine = SYSTEM_STATE_PREFLIGHT; current_status.flag_system_armed = false; + current_status.offboard_control_signal_found_once = false; + current_status.rc_signal_found_once = false; /* advertise to ORB */ stat_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); @@ -1214,8 +1216,16 @@ int commander_thread_main(int argc, char *argv[]) while (!thread_should_exit) { /* Get current values */ - orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man); - orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); + bool new_data; + orb_check(sp_man_sub, &new_data); + if (new_data) { + orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man); + } + + orb_check(sp_offboard_sub, &new_data); + if (new_data) { + orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); + } orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps); orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors); @@ -1385,9 +1395,8 @@ int commander_thread_main(int argc, char *argv[]) /* ignore RC signals if in offboard control mode */ - if (!current_status.offboard_control_signal_found_once) { + if (!current_status.offboard_control_signal_found_once && sp_man.timestamp != 0) { /* Start RC state check */ - bool prev_lost = current_status.rc_signal_lost; if ((hrt_absolute_time() - sp_man.timestamp) < 100000) { @@ -1440,38 +1449,33 @@ int commander_thread_main(int argc, char *argv[]) if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[commander] RECOVERY - RC SIGNAL GAINED!"); } + current_status.rc_signal_cutting_off = false; current_status.rc_signal_lost = false; current_status.rc_signal_lost_interval = 0; } else { static uint64_t last_print_time = 0; /* print error message for first RC glitch and then every 5 s / 5000 ms) */ - if (!current_status.rc_signal_lost || ((hrt_absolute_time() - last_print_time) > 5000000)) { + if (!current_status.rc_signal_cutting_off || ((hrt_absolute_time() - last_print_time) > 5000000)) { /* only complain if the offboard control is NOT active */ + current_status.rc_signal_cutting_off = true; mavlink_log_critical(mavlink_fd, "[commander] CRITICAL - NO REMOTE SIGNAL!"); last_print_time = hrt_absolute_time(); } /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ - current_status.rc_signal_cutting_off = true; current_status.rc_signal_lost_interval = hrt_absolute_time() - sp_man.timestamp; /* if the RC signal is gone for a full second, consider it lost */ if (current_status.rc_signal_lost_interval > 1000000) { current_status.rc_signal_lost = true; current_status.failsave_lowlevel = true; + state_changed = true; } // if (hrt_absolute_time() - current_status.failsave_ll_start_time > failsafe_lowlevel_timeout_ms*1000) { // publish_armed_status(¤t_status); // } } - - /* Check if this is the first loss or first gain*/ - if ((!prev_lost && current_status.rc_signal_lost) || - (prev_lost && !current_status.rc_signal_lost)) { - /* publish change */ - publish_armed_status(¤t_status); - } } @@ -1483,23 +1487,29 @@ int commander_thread_main(int argc, char *argv[]) /* State machine update for offboard control */ - if (!current_status.rc_signal_found_once) { + if (!current_status.rc_signal_found_once && sp_offboard.timestamp != 0) { if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) { + /* decide about attitude control flag, enable in att/pos/vel */ + bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE || + sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || + sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION); + + /* decide about rate control flag, enable it always XXX (for now) */ + bool rates_ctrl_enabled = true; + /* set up control mode */ - if (current_status.flag_control_attitude_enabled != - (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE)) { - current_status.flag_control_attitude_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE); + if (current_status.flag_control_attitude_enabled != attitude_ctrl_enabled) { + current_status.flag_control_attitude_enabled = attitude_ctrl_enabled; state_changed = true; } - if (current_status.flag_control_rates_enabled != - (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES)) { - current_status.flag_control_attitude_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES); + if (current_status.flag_control_rates_enabled != rates_ctrl_enabled) { + current_status.flag_control_rates_enabled = rates_ctrl_enabled; state_changed = true; } - /* handle the case where RC signal was regained */ + /* handle the case where offboard control signal was regained */ if (!current_status.offboard_control_signal_found_once) { current_status.offboard_control_signal_found_once = true; /* enable offboard control, disable manual input */ @@ -1515,6 +1525,7 @@ int commander_thread_main(int argc, char *argv[]) } } + current_status.offboard_control_signal_weak = false; current_status.offboard_control_signal_lost = false; current_status.offboard_control_signal_lost_interval = 0; @@ -1530,8 +1541,8 @@ int commander_thread_main(int argc, char *argv[]) } else { static uint64_t last_print_time = 0; /* print error message for first RC glitch and then every 5 s / 5000 ms) */ - if (!current_status.offboard_control_signal_lost || ((hrt_absolute_time() - last_print_time) > 5000000)) { - /* only complain if the RC control is NOT active */ + if (!current_status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_time) > 5000000)) { + current_status.offboard_control_signal_weak = true; mavlink_log_critical(mavlink_fd, "[commander] CRITICAL - NO OFFBOARD CONTROL SIGNAL!"); last_print_time = hrt_absolute_time(); } diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index c727527b16..23172d7cf1 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -143,6 +143,7 @@ struct vehicle_status_s bool offboard_control_signal_found_once; bool offboard_control_signal_lost; + bool offboard_control_signal_weak; uint64_t offboard_control_signal_lost_interval; /**< interval in microseconds without an offboard control message */ bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */ From d7561fc092955ee766d6d423071c2186efe0a695 Mon Sep 17 00:00:00 2001 From: patacongo Date: Wed, 26 Sep 2012 19:41:54 +0000 Subject: [PATCH 46/56] Shenzhou board has an SSD1289 LCD, not ILI93xx git-svn-id: http://svn.code.sf.net/p/nuttx/code/trunk@5194 42af7a65-404d-4744-a932-0658087f49c3 --- nuttx/ChangeLog | 3 + nuttx/Kconfig | 47 +++-- nuttx/configs/shenzhou/nxwm/defconfig | 6 + nuttx/configs/shenzhou/src/Makefile | 2 +- .../configs/shenzhou/src/shenzhou-internal.h | 11 +- .../shenzhou/src/{up_lcd.c => up_ili93xx.c} | 85 ++++++-- nuttx/configs/shenzhou/src/up_ssd1289.c | 190 +++++++++++++----- nuttx/configs/shenzhou/src/up_touchscreen.c | 2 +- nuttx/drivers/lcd/Kconfig | 28 +++ nuttx/drivers/lcd/ssd1289.c | 8 +- 10 files changed, 286 insertions(+), 96 deletions(-) rename nuttx/configs/shenzhou/src/{up_lcd.c => up_ili93xx.c} (96%) diff --git a/nuttx/ChangeLog b/nuttx/ChangeLog index 3c4f142d12..906b63d33f 100644 --- a/nuttx/ChangeLog +++ b/nuttx/ChangeLog @@ -3413,3 +3413,6 @@ SPI3 and some do not (still not clear). * nuttx/configs/shenzhou: Various fixes to build new NxWM configuration. + * configs/shenzhou: Oops. The Shenzhou LCD is and SSD1289, + not an ILI93xx. + diff --git a/nuttx/Kconfig b/nuttx/Kconfig index 86e8849bc8..7ac512607e 100644 --- a/nuttx/Kconfig +++ b/nuttx/Kconfig @@ -216,7 +216,7 @@ endmenu menu "Debug Options" config DEBUG - bool "Enable debug features" + bool "Enable Debug Features" default n ---help--- Enables built-in debug features. Selecting this option will (1) Enable @@ -227,94 +227,107 @@ config DEBUG if DEBUG config DEBUG_VERBOSE - bool "Enable debug verbose output" + bool "Enable Debug Verbose Output" default n ---help--- Enables verbose debug output (assuming debug output is enabled) config DEBUG_ENABLE - bool "Enable debug controls" + bool "Enable Debug Controls" default n ---help--- Support an interface to dynamically enable or disable debug output. config DEBUG_SCHED - bool "Enable scheduler debug output" + bool "Enable Scheduler Debug Output" default n ---help--- Enable OS debug output (disabled by default) config DEBUG_MM - bool "Enable memory manager debug output" + bool "Enable Memory Manager Debug Output" default n ---help--- Enable memory management debug output (disabled by default) config DEBUG_NET - bool "Enable network debug output" + bool "Enable Network Debug Output" default n + depends on NET ---help--- Enable network debug output (disabled by default) config DEBUG_USB - bool "Enable USB debug output" + bool "Enable USB Debug Output" default n + depends on USBDEV || USBHOST ---help--- Enable usb debug output (disabled by default) config DEBUG_FS - bool "Enable file system debug output" + bool "Enable File System Debug Output" default n ---help--- Enable file system debug output (disabled by default) config DEBUG_LIB - bool "Enable C library debug output" + bool "Enable C Library Debug Output" default n ---help--- Enable C library debug output (disabled by default) config DEBUG_BINFMT - bool "Enable binary loader debug output" + bool "Enable Binary Loader Debug Output" default n ---help--- Enable binary loader debug output (disabled by default) config DEBUG_GRAPHICS - bool "Enable graphics debug output" + bool "Enable Graphics Debug Output" default n ---help--- Enable NX graphics debug output (disabled by default) config DEBUG_LCD - bool "Enable low-leve LCD debug output" + bool "Enable Low-level LCD Debug Output" default n depends on LCD ---help--- Enable low level debug output from the LCD driver (disabled by default) -config DEBUG_I2C - bool "Enable I2C debug output" +config DEBUG_INPUT + bool "Enable Input Device Debug Output" default n + depends on INPUT + ---help--- + Enable low level debug output from the input device drivers such as + mice and touchscreens (disabled by default) + +config DEBUG_I2C + bool "Enable I2C Debug Output" + default n + depends on I2C ---help--- Enable I2C driver debug output (disabled by default) config DEBUG_SPI - bool "Enable SPI debug output" + bool "Enable SPI Debug Output" default n + depends on SPI ---help--- Enable I2C driver debug output (disabled by default) config DEBUG_WATCHDOG - bool "Enable watchdog timer debug output" + bool "Enable Watchdog Timer Debug Output" default n + depends on WATCHDOG ---help--- Enable watchdog timer debug output (disabled by default) endif config DEBUG_SYMBOLS - bool "Enable debug symbols" + bool "Enable Debug Symbols" default n ---help--- Build without optimization and with debug symbols (needed diff --git a/nuttx/configs/shenzhou/nxwm/defconfig b/nuttx/configs/shenzhou/nxwm/defconfig index 9889e887de..29c83730e7 100644 --- a/nuttx/configs/shenzhou/nxwm/defconfig +++ b/nuttx/configs/shenzhou/nxwm/defconfig @@ -197,6 +197,7 @@ CONFIG_STM32_PHYSR_100FD=0x8000 CONFIG_STM32_RMII=y CONFIG_STM32_RMII_MCO=y # CONFIG_STM32_RMII_EXTCLK is not set +# CONFIG_STM32_ETHMAC_REGDEBUG is not set # # USB Host Configuration @@ -350,6 +351,7 @@ CONFIG_SPI_EXCHANGE=y # CONFIG_SPI_CMDDATA is not set CONFIG_RTC=y # CONFIG_RTC_DATETIME is not set +# CONFIG_RTC_HIRES is not set # CONFIG_RTC_ALARM is not set # CONFIG_WATCHDOG is not set # CONFIG_ANALOG is not set @@ -364,6 +366,10 @@ CONFIG_LCD_MAXPOWER=1 # CONFIG_LCD_P14201 is not set # CONFIG_LCD_NOKIA6100 is not set # CONFIG_LCD_UG9664HSWAG01 is not set +CONFIG_LCD_SSD1289=y +CONFIG_SSD1289_PROFILE1=y +# CONFIG_SSD1289_PROFILE2 is not set +# CONFIG_SSD1289_PROFILE3 is not set CONFIG_LCD_LANDSCAPE=y # CONFIG_LCD_PORTRAIT is not set # CONFIG_LCD_RPORTRAIT is not set diff --git a/nuttx/configs/shenzhou/src/Makefile b/nuttx/configs/shenzhou/src/Makefile index 04088110c8..1775f1fcec 100644 --- a/nuttx/configs/shenzhou/src/Makefile +++ b/nuttx/configs/shenzhou/src/Makefile @@ -89,7 +89,7 @@ endif ifeq ($(CONFIG_LCD_SSD1289),y) CSRCS += up_ssd1289.c else -CSRCS += up_lcd.c +CSRCS += up_ili93xx.c endif ifeq ($(CONFIG_INPUT_ADS7843E),y) diff --git a/nuttx/configs/shenzhou/src/shenzhou-internal.h b/nuttx/configs/shenzhou/src/shenzhou-internal.h index 095cba4850..51ab105532 100644 --- a/nuttx/configs/shenzhou/src/shenzhou-internal.h +++ b/nuttx/configs/shenzhou/src/shenzhou-internal.h @@ -295,22 +295,29 @@ (STM32_PERIPHBB_BASE + ((offs + STM32_GPIO_BRR_OFFSET) << 5) + ((pin) << 2)) #define LCD_BIT_SET(offs,pin) \ (STM32_PERIPHBB_BASE + ((offs + STM32_GPIO_BSRR_OFFSET) << 5) + ((pin) << 2)) +#define LCD_BIT_READ(offs,pin) \ + (STM32_PERIPHBB_BASE + ((offs + STM32_GPIO_ODR_OFFSET) << 5) + ((pin) << 2)) #define LCD_RS_CLEAR LCD_BIT_CLEAR(STM32_GPIOD_OFFSET, 13) /* GPIO_PORTD|GPIO_PIN13 */ #define LCD_RS_SET LCD_BIT_SET(STM32_GPIOD_OFFSET, 13) +#define LCD_RS_READ LCD_BIT_READ(STM32_GPIOD_OFFSET, 13) #define LCD_CS_CLEAR LCD_BIT_CLEAR(STM32_GPIOC_OFFSET, 8) /* GPIO_PORTC|GPIO_PIN8 */ #define LCD_CS_SET LCD_BIT_SET(STM32_GPIOC_OFFSET, 8) +#define LCD_CS_READ LCD_BIT_READ(STM32_GPIOC_OFFSET, 8) #define LCD_RD_CLEAR LCD_BIT_CLEAR(STM32_GPIOD_OFFSET, 15) /* GPIO_PORTD|GPIO_PIN15 */ #define LCD_RD_SET LCD_BIT_SET(STM32_GPIOD_OFFSET, 15) +#define LCD_RD_READ LCD_BIT_READ(STM32_GPIOD_OFFSET, 15) #define LCD_WR_CLEAR LCD_BIT_CLEAR(STM32_GPIOD_OFFSET, 14) /* GPIO_PORTD|GPIO_PIN14 */ #define LCD_WR_SET LCD_BIT_SET(STM32_GPIOD_OFFSET, 14) +#define LCD_WR_READ LCD_BIT_READ(STM32_GPIOD_OFFSET, 14) #define LCD_LE_CLEAR LCD_BIT_CLEAR(STM32_GPIOB_OFFSET, 2) /* GPIO_PORTB|GPIO_PIN2 */ #define LCD_LE_SET LCD_BIT_SET(STM32_GPIOB_OFFSET, 2) +#define LCD_LE_READ LCD_BIT_READ(STM32_GPIOB_OFFSET, 2) #define LCD_CRL STM32_GPIOE_CRL #define LCD_CRH STM32_GPIOE_CRH -#define LCD_INPUT 0x44444444 -#define LCD_OUTPUT 0x33333333 +#define LCD_INPUT 0x44444444 /* Floating input */ +#define LCD_OUTPUT 0x33333333 /* Push/pull output */ #define LCD_ODR STM32_GPIOE_ODR #define LCD_IDR STM32_GPIOE_IDR diff --git a/nuttx/configs/shenzhou/src/up_lcd.c b/nuttx/configs/shenzhou/src/up_ili93xx.c similarity index 96% rename from nuttx/configs/shenzhou/src/up_lcd.c rename to nuttx/configs/shenzhou/src/up_ili93xx.c index 1de2a7d4bc..ef2cee417c 100644 --- a/nuttx/configs/shenzhou/src/up_lcd.c +++ b/nuttx/configs/shenzhou/src/up_ili93xx.c @@ -1,6 +1,6 @@ /************************************************************************************ - * configs/shenzhou/src/up_lcd.c - * arch/arm/src/board/up_lcd.c + * configs/shenzhou/src/up_ili93xx.c + * arch/arm/src/board/up_ili93xx.c * * Copyright (C) 2012 Gregory Nutt. All rights reserved. * Authors: Gregory Nutt @@ -211,6 +211,7 @@ # undef CONFIG_DEBUG_VERBOSE # undef CONFIG_DEBUG_GRAPHICS # undef CONFIG_DEBUG_LCD +# undef CONFIG_LCD_REGDEBUG #endif #ifndef CONFIG_DEBUG_VERBOSE @@ -425,6 +426,12 @@ struct stm32_dev_s ************************************************************************************/ /* Low Level LCD access */ +#ifdef CONFIG_LCD_REGDEBUG +static void stm32_lcdshow(FAR struct stm32_lower_s *priv, FAR const char *msg); +#else +# define stm32_lcdshow(p,m) +#endif + static void stm32_writereg(FAR struct stm32_dev_s *priv, uint8_t regaddr, uint16_t regval); static uint16_t stm32_readreg(FAR struct stm32_dev_s *priv, uint8_t regaddr); @@ -485,7 +492,7 @@ static inline void stm32_lcd9919init(FAR struct stm32_dev_s *priv); #ifndef CONFIG_STM32_ILI1505_DISABLE static inline void stm32_lcd1505init(FAR struct stm32_dev_s *priv); #endif -static inline void stm32_lcdinitialize(FAR struct stm32_dev_s *priv); +static inline int stm32_lcdinitialize(FAR struct stm32_dev_s *priv); /************************************************************************************ * Private Data @@ -577,6 +584,33 @@ static struct stm32_dev_s g_lcddev = * Private Functions ************************************************************************************/ +/************************************************************************************ + * Name: stm32_lcdshow + * + * Description: + * Show the state of the interface + * + ************************************************************************************/ + +#ifdef CONFIG_LCD_REGDEBUG +static void stm32_lcdshow(FAR struct stm32_lower_s *priv, FAR const char *msg) +{ + dbg("%s:\n", msg); + dbg(" CRTL RS: %d CS: %d RD: %d WR: %d LE: %d\n", + getreg32(LCD_RS_READ), getreg32(LCD_CS_READ), getreg32(LCD_RD_READ), + getreg32(LCD_WR_READ), getreg32(LCD_LE_READ)); + dbg(" DATA CR: %08x %08x\n", getreg32(LCD_CRL), getreg32(LCD_CRH)); + if (priv->output) + { + dbg(" OUTPUT: %08x\n", getreg32(LCD_ODR)); + } + else + { + dbg(" INPUT: %08x\n", getreg32(LCD_IDR)); + } +} +#endif + /************************************************************************************ * Name: stm32_writereg * @@ -601,7 +635,7 @@ static void stm32_writereg(FAR struct stm32_dev_s *priv, uint8_t regaddr, uint16 /* Then write the 16-bit register value */ - putreg32(1, LCD_RS_CLEAR); + putreg32(1, LCD_RS_SET); putreg32(1, LCD_WR_CLEAR); putreg32((uint32_t)regval, LCD_ODR); putreg32(1, LCD_WR_SET); @@ -638,7 +672,7 @@ static uint16_t stm32_readreg(FAR struct stm32_dev_s *priv, uint8_t regaddr) /* Read the 16-bit register value */ - putreg32(1, LCD_RS_CLEAR); + putreg32(1, LCD_RS_SET); putreg32(1, LCD_RD_CLEAR); putreg32(1, LCD_RD_SET); regval = (uint16_t)getreg32(LCD_IDR); @@ -688,7 +722,7 @@ static inline void stm32_writegram(FAR struct stm32_dev_s *priv, uint16_t rgbval /* Write the value (GRAM register already selected) */ putreg32(1, LCD_CS_CLEAR); - putreg32(1, LCD_RS_CLEAR); + putreg32(1, LCD_RS_SET); putreg32(1, LCD_WR_CLEAR); putreg32((uint32_t)rgbval, LCD_ODR); putreg32(1, LCD_WR_SET); @@ -713,7 +747,8 @@ static inline uint16_t stm32_readgram(FAR struct stm32_dev_s *priv) /* Read the 16-bit value */ - putreg32(1, LCD_RS_CLEAR); + putreg32(1, LCD_CS_CLEAR); + putreg32(1, LCD_RS_SET); putreg32(1, LCD_RD_CLEAR); putreg32(1, LCD_RD_SET); regval = (uint16_t)getreg32(LCD_IDR); @@ -770,8 +805,16 @@ static uint16_t stm32_readnoshift(FAR struct stm32_dev_s *priv, FAR uint16_t *ac static void stm32_setcursor(FAR struct stm32_dev_s *priv, uint16_t col, uint16_t row) { - stm32_writereg(priv, LCD_REG_32, row); /* GRAM horizontal address */ - stm32_writereg(priv, LCD_REG_33, col); /* GRAM vertical address */ + if (priv->type = LCD_TYPE_ILI9919) + { + stm32_writereg(priv, LCD_REG_78, col); /* GRAM horizontal address */ + stm32_writereg(priv, LCD_REG_79, row); /* GRAM vertical address */ + } + else + { + stm32_writereg(priv, LCD_REG_32, row); /* GRAM vertical address */ + stm32_writereg(priv, LCD_REG_33, col); /* GRAM horizontal address */ + } } /************************************************************************************ @@ -1726,9 +1769,10 @@ static inline void stm32_lcd1505init(FAR struct stm32_dev_s *priv) * ************************************************************************************/ -static inline void stm32_lcdinitialize(FAR struct stm32_dev_s *priv) +static inline int stm32_lcdinitialize(FAR struct stm32_dev_s *priv) { uint16_t id; + int ret = OK; /* Check LCD ID */ @@ -1809,11 +1853,12 @@ static inline void stm32_lcdinitialize(FAR struct stm32_dev_s *priv) #endif { lcddbg("Unsupported LCD type\n"); + ret = -ENODEV; } lcddbg("LCD type: %d\n", priv->type); + return ret; } - /************************************************************************************ * Public Functions ************************************************************************************/ @@ -1831,6 +1876,7 @@ static inline void stm32_lcdinitialize(FAR struct stm32_dev_s *priv) int up_lcdinitialize(void) { FAR struct stm32_dev_s *priv = &g_lcddev; + int ret; int i; lcdvdbg("Initializing\n"); @@ -1851,16 +1897,19 @@ int up_lcdinitialize(void) /* Configure and enable LCD */ up_mdelay(50); - stm32_lcdinitialize(priv); + ret = stm32_lcdinitialize(priv); + if (ret == OK) + { + /* Clear the display (setting it to the color 0=black) */ - /* Clear the display (setting it to the color 0=black) */ + stm32_lcdclear(0); - stm32_lcdclear(0); + /* Turn the display off */ - /* Turn the display off */ + stm32_poweroff(priv); + } - stm32_poweroff(priv); - return OK; + return ret; } /************************************************************************************ @@ -1925,7 +1974,7 @@ void stm32_lcdclear(uint16_t color) /* Write the selected color into the entire GRAM memory */ putreg32(1, LCD_CS_CLEAR); - putreg32(1, LCD_RS_CLEAR); + putreg32(1, LCD_RS_SET); for (i = 0; i < STM32_XRES * STM32_YRES; i++) { putreg32(1, LCD_WR_CLEAR); diff --git a/nuttx/configs/shenzhou/src/up_ssd1289.c b/nuttx/configs/shenzhou/src/up_ssd1289.c index 2b48c78ca7..cf02b36d19 100644 --- a/nuttx/configs/shenzhou/src/up_ssd1289.c +++ b/nuttx/configs/shenzhou/src/up_ssd1289.c @@ -76,6 +76,7 @@ # undef CONFIG_DEBUG_VERBOSE # undef CONFIG_DEBUG_GRAPHICS # undef CONFIG_DEBUG_LCD +# undef CONFIG_LCD_REGDEBUG #endif #ifndef CONFIG_DEBUG_VERBOSE @@ -100,14 +101,29 @@ * Private Type Definition ************************************************************************************/ +/* This structure describes the state of this driver */ + +struct stm32_lower_s +{ + struct ssd1289_lcd_s dev; /* This is externally visible the driver state */ + FAR struct lcd_dev_s *drvr; /* The saved instance of the LCD driver */ + bool output; /* True: Configured for output */ +}; + /************************************************************************************** * Private Function Protototypes **************************************************************************************/ /* Helpers */ -static void stm32_wrdata(uint16_t data); +#ifdef CONFIG_LCD_REGDEBUG +static void stm32_lcdshow(FAR struct stm32_lower_s *priv, FAR const char *msg); +#else +# define stm32_lcdshow(p,m) +#endif + +static void stm32_wrdata(FAR struct stm32_lower_s *priv, uint16_t data); #ifndef CONFIG_SSD1289_WRONLY -static uint16_t stm32_rddata(void); +static inline uint16_t stm32_rddata(FAR struct stm32_lower_s *priv); #endif /* Low Level LCD access */ @@ -121,6 +137,11 @@ static uint16_t stm32_read(FAR struct ssd1289_lcd_s *dev); static void stm32_write(FAR struct ssd1289_lcd_s *dev, uint16_t data); static void stm32_backlight(FAR struct ssd1289_lcd_s *dev, int power); +/* Initialization */ + +static void stm32_lcdinput(FAR struct stm32_lower_s *priv); +static void stm32_lcdoutput(FAR struct stm32_lower_s *priv); + /************************************************************************************ * Private Data ************************************************************************************/ @@ -224,28 +245,55 @@ static const uint32_t g_lcdconfig[] = }; #define NLCD_CONFIG (sizeof(g_lcdconfig)/sizeof(uint32_t)) -/* This is the driver state structure (there is no retained state information) */ +/* Driver state structure (only supports one LCD) */ -static struct ssd1289_lcd_s g_ssd1289 = +static struct stm32_lower_s g_lcdlower = { - .select = stm32_select, - .deselect = stm32_deselect, - .index = stm32_index, + { + .select = stm32_select, + .deselect = stm32_deselect, + .index = stm32_index, #ifndef CONFIG_SSD1289_WRONLY - .read = stm32_read, + .read = stm32_read, #endif - .write = stm32_write, - .backlight = stm32_backlight + .write = stm32_write, + .backlight = stm32_backlight + }, + .drvr = NULL, + .output = false }; -/* The saved instance of the LCD driver */ - -static FAR struct lcd_dev_s *g_ssd1289drvr; - /************************************************************************************ * Private Functions ************************************************************************************/ +/************************************************************************************ + * Name: stm32_lcdshow + * + * Description: + * Show the state of the interface + * + ************************************************************************************/ + +#ifdef CONFIG_LCD_REGDEBUG +static void stm32_lcdshow(FAR struct stm32_lower_s *priv, FAR const char *msg) +{ + dbg("%s:\n", msg); + dbg(" CRTL RS: %d CS: %d RD: %d WR: %d LE: %d\n", + getreg32(LCD_RS_READ), getreg32(LCD_CS_READ), getreg32(LCD_RD_READ), + getreg32(LCD_WR_READ), getreg32(LCD_LE_READ)); + dbg(" DATA CR: %08x %08x\n", getreg32(LCD_CRL), getreg32(LCD_CRH)); + if (priv->output) + { + dbg(" OUTPUT: %08x\n", getreg32(LCD_ODR)); + } + else + { + dbg(" INPUT: %08x\n", getreg32(LCD_IDR)); + } +} +#endif + /************************************************************************************ * Name: stm32_wrdata * @@ -254,11 +302,11 @@ static FAR struct lcd_dev_s *g_ssd1289drvr; * ************************************************************************************/ -static void stm32_wrdata(uint16_t data) +static void stm32_wrdata(FAR struct stm32_lower_s *priv, uint16_t data) { /* Make sure D0-D15 are configured as outputs */ - stm32_lcdoutput(); + stm32_lcdoutput(priv); /* Latch the 16-bit LCD data and toggle the WR line */ @@ -276,18 +324,17 @@ static void stm32_wrdata(uint16_t data) ************************************************************************************/ #ifndef CONFIG_SSD1289_WRONLY -static uint16_t stm32_rddata(void); +static inline uint16_t stm32_rddata(FAR struct stm32_lower_s *priv) { /* Make sure D0-D15 are configured as inputs */ - stm32_lcdinput(); + stm32_lcdinput(priv); /* Toggle the RD line to latch the 16-bit LCD data */ -#warning "Requires pins configured as inputs" putreg32(1, LCD_RD_CLEAR); putreg32(1, LCD_RD_SET); - return (uin16_t)getreg32(LCD_IDR); + return (uint16_t)getreg32(LCD_IDR); } #endif @@ -322,7 +369,7 @@ static void stm32_deselect(FAR struct ssd1289_lcd_s *dev) } /************************************************************************************ - * Name: stm32_deselect + * Name: stm32_index * * Description: * Set the index register @@ -331,13 +378,15 @@ static void stm32_deselect(FAR struct ssd1289_lcd_s *dev) static void stm32_index(FAR struct ssd1289_lcd_s *dev, uint8_t index) { - /* Clear the RS signal */ + FAR struct stm32_lower_s *priv = (FAR struct stm32_lower_s *)dev; - putreg32(1, LCD_RS_CLR); + /* Clear the RS signal to select the index address */ + + putreg32(1, LCD_RS_CLEAR); /* And write the index */ - stm32_wrdata((uint16_t)index); + stm32_wrdata(priv, (uint16_t)index); } /************************************************************************************ @@ -351,13 +400,15 @@ static void stm32_index(FAR struct ssd1289_lcd_s *dev, uint8_t index) #ifndef CONFIG_SSD1289_WRONLY static uint16_t stm32_read(FAR struct ssd1289_lcd_s *dev) { - /* Set the RS signal */ + FAR struct stm32_lower_s *priv = (FAR struct stm32_lower_s *)dev; - putreg32(1, LCD_RS_CLR); + /* Set the RS signal to select the data address */ - /* And return the data */ + putreg32(1, LCD_RS_SET); - return stm32_rddata(); + /* Read and return the data */ + + return stm32_rddata(priv); } #endif @@ -371,13 +422,15 @@ static uint16_t stm32_read(FAR struct ssd1289_lcd_s *dev) static void stm32_write(FAR struct ssd1289_lcd_s *dev, uint16_t data) { - /* Set the RS signal */ + FAR struct stm32_lower_s *priv = (FAR struct stm32_lower_s *)dev; - putreg32(1, LCD_RS_CLR); + /* Set the RS signal to select the data address */ + + putreg32(1, LCD_RS_SET); /* And write the data */ - stm32_wrdata(data); + stm32_wrdata(priv, data); } /************************************************************************************ @@ -401,21 +454,31 @@ static void stm32_backlight(FAR struct ssd1289_lcd_s *dev, int power) * ************************************************************************************/ -static void stm32_lcdinput(void) +static void stm32_lcdinput(FAR struct stm32_lower_s *priv) { -#ifdef CONFIG_LCD_FASTCONFIG - putreg32(LCD_INPUT, LCD_CRL); - putreg32(LCD_INPUT, LCD_CRH); -#else +#ifndef CONFIG_LCD_FASTCONFIG int i; - - /* Configure GPIO data lines as inputs */ - - for (i = 0; i < 16; i++) - { - stm32_configgpio(g_lcdin[i]); - } #endif + + /* Check if we are already configured for input */ + + if (priv->output) + { + /* Configure GPIO data lines as inputs */ + +#ifdef CONFIG_LCD_FASTCONFIG + putreg32(LCD_INPUT, LCD_CRL); + putreg32(LCD_INPUT, LCD_CRH); +#else + for (i = 0; i < 16; i++) + { + stm32_configgpio(g_lcdin[i]); + } +#endif + /* No longer configured for output */ + + priv->output = false; + } } /************************************************************************************ @@ -426,15 +489,30 @@ static void stm32_lcdinput(void) * ************************************************************************************/ -static void stm32_lcdoutput(void) +static void stm32_lcdoutput(FAR struct stm32_lower_s *priv) { +#ifndef CONFIG_LCD_FASTCONFIG int i; +#endif - /* Configure GPIO data lines as outputs */ + /* Check if we are already configured for output */ - for (i = 0; i < 16; i++) + if (!priv->output) { - stm32_configgpio(g_lcdout[i]); + /* Configure GPIO data lines as outputs */ + +#ifdef CONFIG_LCD_FASTCONFIG + putreg32(LCD_OUTPUT, LCD_CRL); + putreg32(LCD_OUTPUT, LCD_CRH); +#else + for (i = 0; i < 16; i++) + { + stm32_configgpio(g_lcdout[i]); + } +#endif + /* Now we are configured for output */ + + priv->output = true; } } @@ -454,17 +532,18 @@ static void stm32_lcdoutput(void) int up_lcdinitialize(void) { + FAR struct stm32_lower_s *priv = &g_lcdlower; int i; /* Only initialize the driver once */ - if (!g_ssd1289drvr) + if (!priv->drvr) { lcdvdbg("Initializing\n"); /* Configure GPIO pins */ - stm32_lcdoutput(); + stm32_lcdoutput(priv); for (i = 0; i < NLCD_CONFIG; i++) { stm32_configgpio(g_lcdconfig[i]); @@ -472,8 +551,8 @@ int up_lcdinitialize(void) /* Configure and enable the LCD */ - g_ssd1289drvr = ssd1289_lcdinitialize(&g_ssd1289); - if (!g_ssd1289drvr) + priv->drvr = ssd1289_lcdinitialize(&priv->dev); + if (!priv->drvr) { lcddbg("ERROR: ssd1289_lcdinitialize failed\n"); return -ENODEV; @@ -482,7 +561,7 @@ int up_lcdinitialize(void) /* Turn the display off */ - g_ssd1289drvr->setpower(g_ssd1289drvr, 0); + priv->drvr->setpower(priv->drvr, 0); return OK; } @@ -497,8 +576,9 @@ int up_lcdinitialize(void) FAR struct lcd_dev_s *up_lcdgetdev(int lcddev) { + FAR struct stm32_lower_s *priv = &g_lcdlower; DEBUGASSERT(lcddev == 0); - return g_ssd1289drvr; + return priv->drvr; } /************************************************************************************ @@ -511,9 +591,11 @@ FAR struct lcd_dev_s *up_lcdgetdev(int lcddev) void up_lcduninitialize(void) { + FAR struct stm32_lower_s *priv = &g_lcdlower; + /* Turn the display off */ - g_ssd1289drvr->setpower(g_ssd1289drvr, 0); + priv->drvr->setpower(priv->drvr, 0); } #endif /* CONFIG_LCD_SSD1289 */ diff --git a/nuttx/configs/shenzhou/src/up_touchscreen.c b/nuttx/configs/shenzhou/src/up_touchscreen.c index 92b057f5d4..9f584e5bb9 100644 --- a/nuttx/configs/shenzhou/src/up_touchscreen.c +++ b/nuttx/configs/shenzhou/src/up_touchscreen.c @@ -177,7 +177,7 @@ static void tsc_enable(FAR struct ads7843e_config_s *state, bool enable) /* Attach and enable, or detach and disable */ - ivdbg("IRQ:%d enable:%d\n", STM32_TCS_IRQ, enable); + ivdbg("enable:%d\n", enable); if (enable) { (void)stm32_gpiosetevent(GPIO_TP_INT, false, true, true, diff --git a/nuttx/drivers/lcd/Kconfig b/nuttx/drivers/lcd/Kconfig index 851263d279..640239e637 100644 --- a/nuttx/drivers/lcd/Kconfig +++ b/nuttx/drivers/lcd/Kconfig @@ -43,6 +43,7 @@ config LCD_P14201 p14201.c. Driver for RiT P14201 series display with SD1329 IC controller. This OLED is used with older versions of the TI/Luminary LM3S8962 Evaluation Kit. + if LCD_P14201 config P14201_NINTERFACES int "Number of physical P14201 devices" @@ -189,6 +190,33 @@ config LCD_UG9664HSWAG01 Technology Inc. Used with the LPC Xpresso and Embedded Artists base board. +config LCD_SSD1289 + bool "LCD Based on SSD1289 Controller" + default n + ---help--- + Enables generic support for any LCD based on the Solomon Systech, + Ltd, SSD1289 Controller. Use of this driver will usually require so + detailed customization of the LCD initialization code as necessary + for the specific LCD driven by the SSD1289 controller. + +if LCD_SSD1289 + +choice + prompt "SSD1289 Initialization Profile" + default SSD1289_PROFILE1 + +config SSD1289_PROFILE1 + bool "Profile 1" + +config SSD1289_PROFILE2 + bool "Profile 2" + +config SSD1289_PROFILE3 + bool "Profile 3" + +endchoice +endif + choice prompt "LCD Orientation" default LCD_LANDSCAPE diff --git a/nuttx/drivers/lcd/ssd1289.c b/nuttx/drivers/lcd/ssd1289.c index 58c6069682..3d5ba96d31 100644 --- a/nuttx/drivers/lcd/ssd1289.c +++ b/nuttx/drivers/lcd/ssd1289.c @@ -766,7 +766,7 @@ static int ssd1289_getvideoinfo(FAR struct lcd_dev_s *dev, { DEBUGASSERT(dev && vinfo); lcdvdbg("fmt: %d xres: %d yres: %d nplanes: 1\n", - SSD1289_COLORFMT, SSD1289_XRES, SSD1289_XRES); + SSD1289_COLORFMT, SSD1289_XRES, SSD1289_YRES); vinfo->fmt = SSD1289_COLORFMT; /* Color format: RGB16-565: RRRR RGGG GGGB BBBB */ vinfo->xres = SSD1289_XRES; /* Horizontal resolution in pixel columns */ @@ -925,6 +925,7 @@ static inline int ssd1289_hwinitialize(FAR struct ssd1289_dev_s *priv) #ifndef CONFIG_LCD_NOGETRUN uint16_t id; #endif + int ret; /* Select the LCD */ @@ -1168,19 +1169,20 @@ static inline int ssd1289_hwinitialize(FAR struct ssd1289_dev_s *priv) /* One driver has a 50 msec delay here */ /* up_mdelay(50); */ - return OK; + ret = OK; } #ifndef CONFIG_LCD_NOGETRUN else { lcddbg("Unsupported LCD type\n"); - return -ENODEV; + ret = -ENODEV; } #endif /* De-select the LCD */ lcd->deselect(lcd); + return ret; } /************************************************************************************* From d7456e61ffdf8587973e977a529d297aed233c22 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 26 Sep 2012 22:16:57 +0200 Subject: [PATCH 47/56] Fixed a max value in the AR.Drone interface --- apps/ardrone_interface/ardrone_interface.c | 2 +- apps/ardrone_interface/ardrone_motor_control.c | 4 ++-- apps/ardrone_interface/ardrone_motor_control.h | 3 +++ 3 files changed, 6 insertions(+), 3 deletions(-) diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c index 8d77e7502e..8031497d2f 100644 --- a/apps/ardrone_interface/ardrone_interface.c +++ b/apps/ardrone_interface/ardrone_interface.c @@ -1,7 +1,7 @@ /**************************************************************************** * * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier + * Author: Lorenz Meier * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/apps/ardrone_interface/ardrone_motor_control.c b/apps/ardrone_interface/ardrone_motor_control.c index 787db18773..edb518631d 100644 --- a/apps/ardrone_interface/ardrone_motor_control.c +++ b/apps/ardrone_interface/ardrone_motor_control.c @@ -306,7 +306,7 @@ int ar_init_motors(int ardrone_uart, int gpios) return errcounter; } -/* +/** * Sets the leds on the motor controllers, 1 turns led on, 0 off. */ void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green, uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green) @@ -370,7 +370,7 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls const float min_thrust = 0.02f; /**< 2% minimum thrust */ const float max_thrust = 1.0f; /**< 100% max thrust */ - const float scaling = 512.0f; /**< 100% thrust equals a value of 512 */ + const float scaling = 500.0f; /**< 100% thrust equals a value of 500 which works, 512 leads to cutoff */ const float min_gas = min_thrust * scaling; /**< value range sent to motors, minimum */ const float max_gas = max_thrust * scaling; /**< value range sent to motors, maximum */ diff --git a/apps/ardrone_interface/ardrone_motor_control.h b/apps/ardrone_interface/ardrone_motor_control.h index 664419707a..78b603b63a 100644 --- a/apps/ardrone_interface/ardrone_motor_control.h +++ b/apps/ardrone_interface/ardrone_motor_control.h @@ -87,4 +87,7 @@ int ar_init_motors(int ardrone_uart, int gpio); */ void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green, uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green); +/** + * Mix motors and output actuators + */ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls_s *actuators); From f93464e64fa3bc9d8b5113c67aa690e5bc56d065 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 26 Sep 2012 22:17:13 +0200 Subject: [PATCH 48/56] Fixed RC scaling in sensors app --- apps/sensors/sensors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index eb22ac8a70..3aaef422c9 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -968,7 +968,7 @@ Sensors::ppm_poll() _rc.chan[i].scaled = (ppm_buffer[i] - _parameters.trim[i]) / (float)(_parameters.max[i] - _parameters.trim[i]); } else if (ppm_buffer[i] < (_parameters.trim[i] - _parameters.dz[i])) { /* division by zero impossible for trim == min (as for throttle), as this falls in the above if clause */ - _rc.chan[i].scaled = -1.0f + ((ppm_buffer[i] - _parameters.min[i]) / (float)(_parameters.trim[i] - _parameters.min[i])); + _rc.chan[i].scaled = -((_parameters.trim[i] - ppm_buffer[i]) / (float)(_parameters.trim[i] - _parameters.min[i])); } else { /* in the configured dead zone, output zero */ From ec3949bf82dbaa50ea866b65cd0fc4630af18001 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 26 Sep 2012 22:25:39 +0200 Subject: [PATCH 49/56] Fix a bug where the rate controller is always active --- .../multirotor_att_control_main.c | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 21c720096e..9cb3831b12 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -79,6 +79,8 @@ static bool motor_test_mode = false; static struct actuator_controls_s actuators; static orb_advert_t actuator_pub; +static struct vehicle_status_s state; + /** * Perform rate control right after gyro reading */ @@ -118,7 +120,7 @@ static void *rate_control_thread_main(void *arg) /* perform local lowpass */ /* apply controller */ - // if (state.flag_control_rates_enabled) { + if (state.flag_control_rates_enabled) { /* lowpass gyros */ // XXX gyro_lp[0] = gyro_report.x; @@ -127,7 +129,7 @@ static void *rate_control_thread_main(void *arg) multirotor_control_rates(&rates_sp, gyro_lp, &actuators); orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - // } + } } } @@ -138,7 +140,6 @@ static int mc_thread_main(int argc, char *argv[]) { /* declare and safely initialize all structs */ - struct vehicle_status_s state; memset(&state, 0, sizeof(state)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); @@ -201,7 +202,11 @@ mc_thread_main(int argc, char *argv[]) perf_begin(mc_loop_perf); /* get a local copy of system state */ - orb_copy(ORB_ID(vehicle_status), state_sub, &state); + bool updated; + orb_check(state_sub, &updated); + if (updated) { + orb_copy(ORB_ID(vehicle_status), state_sub, &state); + } /* get a local copy of manual setpoint */ orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); /* get a local copy of attitude */ @@ -209,7 +214,6 @@ mc_thread_main(int argc, char *argv[]) /* get a local copy of attitude setpoint */ orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp); /* get a local copy of rates setpoint */ - bool updated; orb_check(setpoint_sub, &updated); if (updated) { orb_copy(ORB_ID(offboard_control_setpoint), setpoint_sub, &offboard_sp); From 83f66b9e182d4ea8144f936f3012ba78ce14cf3a Mon Sep 17 00:00:00 2001 From: patacongo Date: Wed, 26 Sep 2012 22:03:53 +0000 Subject: [PATCH 50/56] Shenzhou schematic is wrong: LCD WR signal is on PB14, not PD14 git-svn-id: http://svn.code.sf.net/p/nuttx/code/trunk@5195 42af7a65-404d-4744-a932-0658087f49c3 --- nuttx/ChangeLog | 2 ++ nuttx/Kconfig | 4 ++++ nuttx/configs/shenzhou/README.txt | 4 ++-- nuttx/configs/shenzhou/src/shenzhou-internal.h | 12 ++++++------ nuttx/configs/shenzhou/src/up_ili93xx.c | 2 +- nuttx/configs/shenzhou/src/up_ssd1289.c | 14 ++++++++++++-- 6 files changed, 27 insertions(+), 11 deletions(-) diff --git a/nuttx/ChangeLog b/nuttx/ChangeLog index 906b63d33f..80d889b3b5 100644 --- a/nuttx/ChangeLog +++ b/nuttx/ChangeLog @@ -3415,4 +3415,6 @@ configuration. * configs/shenzhou: Oops. The Shenzhou LCD is and SSD1289, not an ILI93xx. + * configs/shenzhou/src/up_ssd1289.c: The LCD is basically functional + on the Shenzhou board. diff --git a/nuttx/Kconfig b/nuttx/Kconfig index 7ac512607e..f4f6abe7e0 100644 --- a/nuttx/Kconfig +++ b/nuttx/Kconfig @@ -238,6 +238,8 @@ config DEBUG_ENABLE ---help--- Support an interface to dynamically enable or disable debug output. +comment "Subsystem Debug Options" + config DEBUG_SCHED bool "Enable Scheduler Debug Output" default n @@ -288,6 +290,8 @@ config DEBUG_GRAPHICS ---help--- Enable NX graphics debug output (disabled by default) +comment "Driver Debug Options" + config DEBUG_LCD bool "Enable Low-level LCD Debug Output" default n diff --git a/nuttx/configs/shenzhou/README.txt b/nuttx/configs/shenzhou/README.txt index b083f293ec..833da5bd1d 100644 --- a/nuttx/configs/shenzhou/README.txt +++ b/nuttx/configs/shenzhou/README.txt @@ -94,7 +94,7 @@ PN NAME SIGNAL NOTES MII_TXD0 Ethernet PHY 52 PB13 I2S_CK Audio DAC MII_TXD1 Ethernet PHY -53 PB14 SD_CD +53 PB14 SD_CD There is confusion here. Schematic is wrong LCD_WR is PB14. 54 PB15 I2S_DIN Audio DAC -- ---- -------------- ------------------------------------------------------------------- @@ -139,7 +139,7 @@ PN NAME SIGNAL NOTES 58 PD11 SD_CS Active low: Pulled high (See also TFT LCD CN13, pin 32) 59 PD12 WIRELESS_CS To the NRF24L01 2.4G wireless module 60 PD13 LCD_RS To TFT LCD (CN13) -61 PD14 LCD_WR To TFT LCD (CN13) +61 PD14 LCD_WR To TFT LCD (CN13). Schematic is wrong LCD_WR is PB14. 62 PD15 LCD_RD To TFT LCD (CN13) -- ---- -------------- ------------------------------------------------------------------- diff --git a/nuttx/configs/shenzhou/src/shenzhou-internal.h b/nuttx/configs/shenzhou/src/shenzhou-internal.h index 51ab105532..f9cebc5e38 100644 --- a/nuttx/configs/shenzhou/src/shenzhou-internal.h +++ b/nuttx/configs/shenzhou/src/shenzhou-internal.h @@ -164,7 +164,7 @@ * 80 PC12 SPI3_MOSI To TFT LCD (CN13, pin 27) * 58 PD11 SD_CS Active low: Pulled high (See also TFT LCD CN13, pin 32) * 60 PD13 LCD_RS To TFT LCD (CN13, pin 20) - * 61 PD14 LCD_WR To TFT LCD (CN13, pin 21) + * 61 PD14 LCD_WR To TFT LCD (CN13, pin 21). Schematic is wrong LCD_WR is PB14. * 62 PD15 LCD_RD To TFT LCD (CN13, pin 22) * 97 PE0 DB00 To TFT LCD (CN13, pin 3) * 98 PE1 DB01 To TFT LCD (CN13, pin 4) @@ -281,7 +281,7 @@ #define GPIO_LCD_RD (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN15) #define GPIO_LCD_WR (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ - GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN14) + GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14) #define GPIO_LCD_LE (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN2) @@ -307,9 +307,9 @@ #define LCD_RD_CLEAR LCD_BIT_CLEAR(STM32_GPIOD_OFFSET, 15) /* GPIO_PORTD|GPIO_PIN15 */ #define LCD_RD_SET LCD_BIT_SET(STM32_GPIOD_OFFSET, 15) #define LCD_RD_READ LCD_BIT_READ(STM32_GPIOD_OFFSET, 15) -#define LCD_WR_CLEAR LCD_BIT_CLEAR(STM32_GPIOD_OFFSET, 14) /* GPIO_PORTD|GPIO_PIN14 */ -#define LCD_WR_SET LCD_BIT_SET(STM32_GPIOD_OFFSET, 14) -#define LCD_WR_READ LCD_BIT_READ(STM32_GPIOD_OFFSET, 14) +#define LCD_WR_CLEAR LCD_BIT_CLEAR(STM32_GPIOB_OFFSET, 14) /* GPIO_PORTB|GPIO_PIN14 */ +#define LCD_WR_SET LCD_BIT_SET(STM32_GPIOB_OFFSET, 14) +#define LCD_WR_READ LCD_BIT_READ(STM32_GPIOB_OFFSET, 14) #define LCD_LE_CLEAR LCD_BIT_CLEAR(STM32_GPIOB_OFFSET, 2) /* GPIO_PORTB|GPIO_PIN2 */ #define LCD_LE_SET LCD_BIT_SET(STM32_GPIOB_OFFSET, 2) #define LCD_LE_READ LCD_BIT_READ(STM32_GPIOB_OFFSET, 2) @@ -383,7 +383,7 @@ * -- ---- -------------- ------------------------------------------------------------------- * PN NAME SIGNAL NOTES * -- ---- -------------- ------------------------------------------------------------------- - * 53 PB14 SD_CD Active low: Pulled high + * 53 PB14 SD_CD Active low: Pulled high. Schematic is wrong LCD_WR is PB14. * 58 PD11 SD_CS Active low: Pulled high (See also TFT LCD CN13, pin 32) */ diff --git a/nuttx/configs/shenzhou/src/up_ili93xx.c b/nuttx/configs/shenzhou/src/up_ili93xx.c index ef2cee417c..d3e2291da7 100644 --- a/nuttx/configs/shenzhou/src/up_ili93xx.c +++ b/nuttx/configs/shenzhou/src/up_ili93xx.c @@ -49,7 +49,7 @@ * 80 PC12 SPI3_MOSI To TFT LCD (CN13, pin 27) * 58 PD11 SD_CS Active low: Pulled high (See also TFT LCD CN13, pin 32) * 60 PD13 LCD_RS To TFT LCD (CN13, pin 20) - * 61 PD14 LCD_WR To TFT LCD (CN13, pin 21) + * 61 PD14 LCD_WR To TFT LCD (CN13, pin 21). Schematic is wrong LCD_WR is PB14. * 62 PD15 LCD_RD To TFT LCD (CN13, pin 22) * 97 PE0 DB00 To TFT LCD (CN13, pin 3) * 98 PE1 DB01 To TFT LCD (CN13, pin 4) diff --git a/nuttx/configs/shenzhou/src/up_ssd1289.c b/nuttx/configs/shenzhou/src/up_ssd1289.c index cf02b36d19..00e0ac0ddc 100644 --- a/nuttx/configs/shenzhou/src/up_ssd1289.c +++ b/nuttx/configs/shenzhou/src/up_ssd1289.c @@ -160,7 +160,7 @@ static void stm32_lcdoutput(FAR struct stm32_lower_s *priv); * 80 PC12 SPI3_MOSI To TFT LCD (CN13, pin 27) * 58 PD11 SD_CS Active low: Pulled high (See also TFT LCD CN13, pin 32) * 60 PD13 LCD_RS To TFT LCD (CN13, pin 20) - * 61 PD14 LCD_WR To TFT LCD (CN13, pin 21) + * 61 PD14 LCD_WR To TFT LCD (CN13, pin 21). Schematic is wrong LCD_WR is PB14. * 62 PD15 LCD_RD To TFT LCD (CN13, pin 22) * 97 PE0 DB00 To TFT LCD (CN13, pin 3) * 98 PE1 DB01 To TFT LCD (CN13, pin 4) @@ -312,6 +312,9 @@ static void stm32_wrdata(FAR struct stm32_lower_s *priv, uint16_t data) putreg32(1, LCD_WR_CLEAR); putreg32((uint32_t)data, LCD_ODR); + + /* Total WR pulse with should be 50ns wide. */ + putreg32(1, LCD_WR_SET); } @@ -326,6 +329,8 @@ static void stm32_wrdata(FAR struct stm32_lower_s *priv, uint16_t data) #ifndef CONFIG_SSD1289_WRONLY static inline uint16_t stm32_rddata(FAR struct stm32_lower_s *priv) { + uint16_t regval; + /* Make sure D0-D15 are configured as inputs */ stm32_lcdinput(priv); @@ -333,8 +338,13 @@ static inline uint16_t stm32_rddata(FAR struct stm32_lower_s *priv) /* Toggle the RD line to latch the 16-bit LCD data */ putreg32(1, LCD_RD_CLEAR); + + /* Data should appear 250ns after RD. Total RD pulse width should be 500nS */ + + __asm__ __volatile__(" nop\n nop\n nop\n nop\n"); + regval = (uint16_t)getreg32(LCD_IDR); putreg32(1, LCD_RD_SET); - return (uint16_t)getreg32(LCD_IDR); + return regval; } #endif From ebdc6a36e8185325a98c45527b2c970aae5a59e2 Mon Sep 17 00:00:00 2001 From: patacongo Date: Thu, 27 Sep 2012 01:26:47 +0000 Subject: [PATCH 51/56] Fixes mostly related to touchscreen on Shenzhou board (still does not work) git-svn-id: http://svn.code.sf.net/p/nuttx/code/trunk@5196 42af7a65-404d-4744-a932-0658087f49c3 --- NxWidgets/libnxwidgets/src/cnxserver.cxx | 2 +- NxWidgets/nxwm/src/chexcalculator.cxx | 2 +- nuttx/configs/shenzhou/README.txt | 58 +++++++++++++++++++++ nuttx/configs/shenzhou/src/up_touchscreen.c | 17 ++++-- nuttx/drivers/input/Kconfig | 9 +++- nuttx/drivers/input/ads7843e.c | 9 +++- nuttx/drivers/input/ads7843e.h | 6 +++ 7 files changed, 94 insertions(+), 9 deletions(-) diff --git a/NxWidgets/libnxwidgets/src/cnxserver.cxx b/NxWidgets/libnxwidgets/src/cnxserver.cxx index be67e56412..8816c73bc6 100644 --- a/NxWidgets/libnxwidgets/src/cnxserver.cxx +++ b/NxWidgets/libnxwidgets/src/cnxserver.cxx @@ -214,7 +214,7 @@ bool CNxServer::connect(void) // Start the server task - gvdbg("NxServer::connect: Starting server task\n"); + gvdbg("CNxServer::connect: Starting server task\n"); serverId = TASK_CREATE("NX Server", CONFIG_NXWIDGETS_SERVERPRIO, CONFIG_NXWIDGETS_SERVERSTACK, server, (FAR const char **)0); if (serverId < 0) diff --git a/NxWidgets/nxwm/src/chexcalculator.cxx b/NxWidgets/nxwm/src/chexcalculator.cxx index 167c114b70..f8f1bb246a 100644 --- a/NxWidgets/nxwm/src/chexcalculator.cxx +++ b/NxWidgets/nxwm/src/chexcalculator.cxx @@ -656,7 +656,7 @@ int64_t CHexCalculator::evaluateBinaryOperation(uint8_t operation, int64_t value return value1 + value2; default: - gdbg("ERROR: Unexpected pending operation %d\n", m_pending); + gdbg("ERROR: Unexpected pending operation %d\n", operation); return 0; } } diff --git a/nuttx/configs/shenzhou/README.txt b/nuttx/configs/shenzhou/README.txt index 833da5bd1d..9c358c5a5d 100644 --- a/nuttx/configs/shenzhou/README.txt +++ b/nuttx/configs/shenzhou/README.txt @@ -696,3 +696,61 @@ Where is one of the following: delay (maybe 30 seconds?) before anything happens. That is the timeout before the networking finally gives up and decides that no network is available. + + nxwm + ---- + This is a special configuration setup for the NxWM window manager + UnitTest. The NxWM window manager can be found here: + + trunk/NxWidgets/nxwm + + The NxWM unit test can be found at: + + trunk/NxWidgets/UnitTests/nxwm + + NOTE: JP6 selects between the touchscreen interrupt and the MII + interrupt. It should be positioned 1-2 to enable the touchscreen + interrupt. + + Documentation for installing the NxWM unit test can be found here: + + trunk/NxWidgets/UnitTests/README.txt + + Here is the quick summary of the build steps: + + 1. Intall the nxwm configuration + + $ cd ~/nuttx/trunk/nuttx/tools + $ ./configure.sh shenzhou/nxwm + + 2. Make the build context (only) + + $ cd .. + $ . ./setenv.sh + $ make context + ... + + 3. Install the nxwm unit test + + $ cd ~/nuttx/trunk/NxWidgets + $ tools/install.sh ~/nuttx/trunk/apps nxwm + Creating symbolic link + - To ~/nuttx/trunk/NxWidgets/UnitTests/nxwm + - At ~/nuttx/trunk/apps/external + + 4. Build the NxWidgets library + + $ cd ~/nuttx/trunk/NxWidgets/libnxwidgets + $ make TOPDIR=~/nuttx/trunk/nuttx + ... + + 5. Build the NxWM library + + $ cd ~/nuttx/trunk/NxWidgets/nxwm + $ make TOPDIR=~//nuttx/trunk/nuttx + ... + + 6. Built NuttX with the installed unit test as the application + + $ cd ~/nuttx/trunk/nuttx + $ make diff --git a/nuttx/configs/shenzhou/src/up_touchscreen.c b/nuttx/configs/shenzhou/src/up_touchscreen.c index 9f584e5bb9..7f6d49eb5b 100644 --- a/nuttx/configs/shenzhou/src/up_touchscreen.c +++ b/nuttx/configs/shenzhou/src/up_touchscreen.c @@ -173,19 +173,23 @@ static void tsc_enable(FAR struct ads7843e_config_s *state, bool enable) { FAR struct stm32_config_s *priv = (FAR struct stm32_config_s *)state; - DEBUGASSERT(priv->handler); + /* The caller should not attempt to enable interrupts if the handler + * has not yet been 'attached' + */ + + DEBUGASSERT(priv->handler || !enable); /* Attach and enable, or detach and disable */ ivdbg("enable:%d\n", enable); if (enable) { - (void)stm32_gpiosetevent(GPIO_TP_INT, false, true, true, + (void)stm32_gpiosetevent(GPIO_TP_INT, true, true, false, priv->handler); } else { - (void)stm32_gpiosetevent(GPIO_TP_INT, false, true, true, NULL); + (void)stm32_gpiosetevent(GPIO_TP_INT, false, false, false, NULL); } } @@ -205,9 +209,12 @@ static bool tsc_busy(FAR struct ads7843e_config_s *state) static bool tsc_pendown(FAR struct ads7843e_config_s *state) { - /* REVISIT: This might need to be inverted */ + /* XPT2046 uses an an internal pullup resistor. The PENIRQ output goes low + * due to the current path through the touch screen to ground, which + * initiates an interrupt to the processor via TP_INT. + */ - bool pendown = stm32_gpioread(GPIO_TP_INT); + bool pendown = !stm32_gpioread(GPIO_TP_INT); ivdbg("pendown:%d\n", pendown); return pendown; } diff --git a/nuttx/drivers/input/Kconfig b/nuttx/drivers/input/Kconfig index 1303cfbd0f..9fde35ff67 100644 --- a/nuttx/drivers/input/Kconfig +++ b/nuttx/drivers/input/Kconfig @@ -6,8 +6,15 @@ config INPUT_TSC2007 bool "TI TSC2007 touchscreen controller" default n select I2C + ---help--- + Enable support for the TI TSC2007 touchscreen controller + config INPUT_ADS7843E - bool "TI ADS7843E touchscreen controller" + bool "TI ADS7843/TSC2046 touchscreen controller" default n select SPI + ---help--- + Enable support for the TI/Burr-Brown ADS7842 touchscreen controller. I believe + that driver should be compatibile with the TI/Burr-Brown TSC2046 and XPT2046 + touchscreen controllers as well. diff --git a/nuttx/drivers/input/ads7843e.c b/nuttx/drivers/input/ads7843e.c index e08a7a7280..555ce34806 100644 --- a/nuttx/drivers/input/ads7843e.c +++ b/nuttx/drivers/input/ads7843e.c @@ -9,6 +9,12 @@ * "Touch Screen Controller, ADS7843," Burr-Brown Products from Texas * Instruments, SBAS090B, September 2000, Revised May 2002" * + * See also: + * "Low Voltage I/O Touch Screen Controller, TSC2046," Burr-Brown Products + * from Texas Instruments, SBAS265F, October 2002, Revised August 2007. + * + * "XPT2046 Data Sheet," Shenzhen XPTek Technology Co., Ltd, 2007 + * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: @@ -566,7 +572,7 @@ static void ads7843e_worker(FAR void *arg) /* Check for pen up or down by reading the PENIRQ GPIO. */ pendown = config->pendown(config); - +dbg("pendown: %d\n", pendown); // REMOVE ME /* Handle the change from pen down to pen up */ if (!pendown) @@ -637,6 +643,7 @@ static void ads7843e_worker(FAR void *arg) /* Exit, re-enabling ADS7843E interrupts */ errout: +dbg("Exiting\n"); // REMOVE ME (void)ads7843e_sendcmd(priv, ADS7843_CMD_ENABPINIRQ); config->enable(config, true); } diff --git a/nuttx/drivers/input/ads7843e.h b/nuttx/drivers/input/ads7843e.h index 030d1cb332..43b79c7b7d 100644 --- a/nuttx/drivers/input/ads7843e.h +++ b/nuttx/drivers/input/ads7843e.h @@ -8,6 +8,12 @@ * "Touch Screen Controller, ADS7843," Burr-Brown Products from Texas * Instruments, SBAS090B, September 2000, Revised May 2002" * + * See also: + * "Low Voltage I/O Touch Screen Controller, TSC2046," Burr-Brown Products + * from Texas Instruments, SBAS265F, October 2002, Revised August 2007." + * + * "XPT2046 Data Sheet," Shenzhen XPTek Technology Co., Ltd, 2007 + * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: From 9a0be89531cef3fb41ac68412693529607dd0ca3 Mon Sep 17 00:00:00 2001 From: patacongo Date: Thu, 27 Sep 2012 03:13:50 +0000 Subject: [PATCH 52/56] Correct some bad parameter checking git-svn-id: http://svn.code.sf.net/p/nuttx/code/trunk@5197 42af7a65-404d-4744-a932-0658087f49c3 --- nuttx/ChangeLog | 2 ++ nuttx/Kconfig | 6 +++++- nuttx/drivers/input/ads7843e.c | 3 +-- nuttx/graphics/nxmu/nx_block.c | 4 ++-- nuttx/graphics/nxmu/nxmu_sendclient.c | 4 ++-- nuttx/graphics/nxmu/nxmu_sendserver.c | 4 ++-- 6 files changed, 14 insertions(+), 9 deletions(-) diff --git a/nuttx/ChangeLog b/nuttx/ChangeLog index 80d889b3b5..adb52afce7 100644 --- a/nuttx/ChangeLog +++ b/nuttx/ChangeLog @@ -3417,4 +3417,6 @@ not an ILI93xx. * configs/shenzhou/src/up_ssd1289.c: The LCD is basically functional on the Shenzhou board. + * graphics/nxmu: Correct some bad parameter checking that caused + failures when DEBUG was enabled. diff --git a/nuttx/Kconfig b/nuttx/Kconfig index f4f6abe7e0..0fe6eb0f7c 100644 --- a/nuttx/Kconfig +++ b/nuttx/Kconfig @@ -230,7 +230,11 @@ config DEBUG_VERBOSE bool "Enable Debug Verbose Output" default n ---help--- - Enables verbose debug output (assuming debug output is enabled) + Enables verbose debug output (assuming debug output is enabled). As a + general rule, when DEBUG is enabled only errors will be reported in the debug + output. But if you also enable DEBUG_VERBOSE, then very chatty (and + often annoying) output will be generated. This means there are two levels + of debug output: errors-only and everything. config DEBUG_ENABLE bool "Enable Debug Controls" diff --git a/nuttx/drivers/input/ads7843e.c b/nuttx/drivers/input/ads7843e.c index 555ce34806..07e5e515df 100644 --- a/nuttx/drivers/input/ads7843e.c +++ b/nuttx/drivers/input/ads7843e.c @@ -572,7 +572,7 @@ static void ads7843e_worker(FAR void *arg) /* Check for pen up or down by reading the PENIRQ GPIO. */ pendown = config->pendown(config); -dbg("pendown: %d\n", pendown); // REMOVE ME + /* Handle the change from pen down to pen up */ if (!pendown) @@ -643,7 +643,6 @@ dbg("pendown: %d\n", pendown); // REMOVE ME /* Exit, re-enabling ADS7843E interrupts */ errout: -dbg("Exiting\n"); // REMOVE ME (void)ads7843e_sendcmd(priv, ADS7843_CMD_ENABPINIRQ); config->enable(config, true); } diff --git a/nuttx/graphics/nxmu/nx_block.c b/nuttx/graphics/nxmu/nx_block.c index 2f069f0962..3a051f9d7d 100644 --- a/nuttx/graphics/nxmu/nx_block.c +++ b/nuttx/graphics/nxmu/nx_block.c @@ -111,8 +111,8 @@ int nx_block(NXWINDOW hwnd, FAR void *arg) #ifdef CONFIG_DEBUG if (!hwnd) { - errno = EINVAL; - return NULL; + set_errno(EINVAL); + return ERROR; } #endif diff --git a/nuttx/graphics/nxmu/nxmu_sendclient.c b/nuttx/graphics/nxmu/nxmu_sendclient.c index 8b7f121042..a59fa23634 100644 --- a/nuttx/graphics/nxmu/nxmu_sendclient.c +++ b/nuttx/graphics/nxmu/nxmu_sendclient.c @@ -93,9 +93,9 @@ int nxmu_sendclient(FAR struct nxfe_conn_s *conn, FAR const void *msg, /* Sanity checking */ #ifdef CONFIG_DEBUG - if (!conn || conn->swrmq) + if (!conn || !conn->swrmq) { - errno = EINVAL; + set_errno(EINVAL); return ERROR; } #endif diff --git a/nuttx/graphics/nxmu/nxmu_sendserver.c b/nuttx/graphics/nxmu/nxmu_sendserver.c index 7007b81da9..03ece4e7f7 100644 --- a/nuttx/graphics/nxmu/nxmu_sendserver.c +++ b/nuttx/graphics/nxmu/nxmu_sendserver.c @@ -93,9 +93,9 @@ int nxmu_sendserver(FAR struct nxfe_conn_s *conn, FAR const void *msg, /* Sanity checking */ #ifdef CONFIG_DEBUG - if (!conn || conn->cwrmq) + if (!conn || !conn->cwrmq) { - errno = EINVAL; + set_errno(EINVAL); return ERROR; } #endif From 7f153098926bf977609c6efb53fa7cb5093564af Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 27 Sep 2012 16:50:20 +0200 Subject: [PATCH 53/56] Calibration should not freeze anymore, ardrone flying but estimator is not able to use calibrated magnetometer data --- apps/ardrone_interface/ardrone_interface.c | 3 +- apps/commander/commander.c | 48 +++++++------------ .../multirotor_att_control_main.c | 10 ++-- 3 files changed, 23 insertions(+), 38 deletions(-) diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c index f0bc0b1e78..8b4b5c400f 100644 --- a/apps/ardrone_interface/ardrone_interface.c +++ b/apps/ardrone_interface/ardrone_interface.c @@ -140,9 +140,8 @@ static int ardrone_open_uart(char *uart_name, struct termios *uart_config_origin int speed = B115200; int uart; - /* open uart */ - printf("[ardrone_interface] UART is %s, baud rate is%d\n",uart_name,speed); + //printf("[ardrone_interface] UART is %s, baud rate is%d\n",uart_name,speed); uart = open(uart_name, O_RDWR | O_NOCTTY); /* Try to set baud rate */ diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 66a981a685..a1c2a15d26 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -312,8 +312,6 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) const uint64_t calibration_interval_us = 45 * 1000000; unsigned int calibration_counter = 0; - const int peak_samples = 500; - float mag_max[3] = {0, 0, 0}; float mag_min[3] = {0, 0, 0}; @@ -366,8 +364,8 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) calibration_counter++; } else { /* any poll failure for 1s is a reason to abort */ - //mavlink_log_info(mavlink_fd, "[commander] mag calibration aborted, please retry."); - //break; + mavlink_log_info(mavlink_fd, "[commander] mag calibration aborted, please retry."); + break; } } @@ -389,20 +387,20 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) * offset = (max + min) / 2.0f */ -// printf("max 0: %f\n",mag_max[0]); -// printf("max 1: %f\n",mag_max[1]); -// printf("max 2: %f\n",mag_max[2]); -// printf("min 0: %f\n",mag_min[0]); -// printf("min 1: %f\n",mag_min[1]); -// printf("min 2: %f\n",mag_min[2]); + printf("max 0: %f\n",mag_max[0]); + printf("max 1: %f\n",mag_max[1]); + printf("max 2: %f\n",mag_max[2]); + printf("min 0: %f\n",mag_min[0]); + printf("min 1: %f\n",mag_min[1]); + printf("min 2: %f\n",mag_min[2]); mag_offset[0] = (mag_max[0] + mag_min[0]) / 2.0f; mag_offset[1] = (mag_max[1] + mag_min[1]) / 2.0f; mag_offset[2] = (mag_max[2] + mag_min[2]) / 2.0f; -// printf("mag off 0: %f\n",mag_offset[0]); -// printf("mag off 1: %f\n",mag_offset[1]); -// printf("mag off 2: %f\n",mag_offset[2]); + printf("mag off 0: %f\n",mag_offset[0]); + printf("mag off 1: %f\n",mag_offset[1]); + printf("mag off 2: %f\n",mag_offset[2]); /* announce and set new offset */ @@ -487,7 +485,7 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) calibration_counter++; } else { /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "[commander] gyro calibration aborted, please retry."); + mavlink_log_info(mavlink_fd, "[commander] gyro calibration aborted, retry"); return; } } @@ -534,9 +532,7 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) mavlink_log_info(mavlink_fd, "[commander] gyro calibration finished"); - // char offset_output[50]; - // sprintf(offset_output, "[commander] gyro cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]); - // mavlink_log_info(mavlink_fd, offset_output); + printf("[commander] gyro cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]); close(sub_sensor_combined); } @@ -544,9 +540,8 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) void do_accel_calibration(int status_pub, struct vehicle_status_s *status) { /* announce change */ - usleep(5000); - mavlink_log_info(mavlink_fd, "[commander] The system should be level and not moved"); + mavlink_log_info(mavlink_fd, "[commander] keep it level and still"); /* set to accel calibration mode */ status->flag_preflight_accel_calibration = true; state_machine_publish(status_pub, status, mavlink_fd); @@ -571,7 +566,6 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale_null)) warn("WARNING: failed to set scale / offsets for accel"); close(fd); - while (calibration_counter < calibration_count) { /* wait blocking for new data */ @@ -585,11 +579,10 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) calibration_counter++; } else { /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "[commander] gyro calibration aborted, please retry."); + mavlink_log_info(mavlink_fd, "[commander] acceleration calibration aborted"); return; } } - accel_offset[0] = accel_offset[0] / calibration_count; accel_offset[1] = accel_offset[1] / calibration_count; accel_offset[2] = accel_offset[2] / calibration_count; @@ -644,18 +637,11 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) if(save_ret != 0) { warn("WARNING: auto-save of params to EEPROM failed"); } - /* exit to gyro calibration mode */ status->flag_preflight_accel_calibration = false; state_machine_publish(status_pub, status, mavlink_fd); - mavlink_log_info(mavlink_fd, "[commander] acceleration calibration finished"); - - // char offset_output[50]; - // sprintf(offset_output, "[commander] accel cal: x:%8.4f y:%8.4f z:%8.4f", (double)accel_offset[0], - // (double)accel_offset[1], (double)accel_offset[2]); - // mavlink_log_info(mavlink_fd, offset_output); - + printf("[commander] accel calibration: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0],(double)accel_offset[1], (double)accel_offset[2]); close(sub_sensor_combined); } @@ -823,7 +809,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* none found */ if (!handled) { //fprintf(stderr, "[commander] refusing unsupported calibration request\n"); - mavlink_log_critical(mavlink_fd, "[commander] CMD refusing unsupported calibration request"); + mavlink_log_critical(mavlink_fd, "[commander] CMD refusing unsup. calib. request"); result = MAV_RESULT_UNSUPPORTED; } } diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 8d648c1944..b00b6bc0ca 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -193,10 +193,10 @@ mc_thread_main(int argc, char *argv[]) /* ready, spawn pthread */ pthread_attr_t rate_control_attr; -// pthread_attr_init(&rate_control_attr); -// pthread_attr_setstacksize(&rate_control_attr, 2048); -// pthread_t rate_control_thread; -// pthread_create(&rate_control_thread, &rate_control_attr, rate_control_thread_main, NULL); + pthread_attr_init(&rate_control_attr); + pthread_attr_setstacksize(&rate_control_attr, 2048); + pthread_t rate_control_thread; + pthread_create(&rate_control_thread, &rate_control_attr, rate_control_thread_main, NULL); while (!thread_should_exit) { @@ -310,7 +310,7 @@ mc_thread_main(int argc, char *argv[]) perf_print_counter(mc_loop_perf); perf_free(mc_loop_perf); - //pthread_join(rate_control_thread, NULL); + pthread_join(rate_control_thread, NULL); fflush(stdout); exit(0); From 2c5c3141057be1f46cb4a33f71e2331ce36b18a7 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 27 Sep 2012 17:08:29 +0200 Subject: [PATCH 54/56] Cleanup of lots of debugging printfs --- apps/ardrone_interface/ardrone_interface.c | 11 ++--------- apps/ardrone_interface/ardrone_motor_control.c | 4 ---- apps/commander/commander.c | 4 ---- .../multirotor_att_control_main.c | 6 ------ .../multirotor_attitude_control.c | 7 ------- apps/sensors/sensors.cpp | 1 - 6 files changed, 2 insertions(+), 31 deletions(-) diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c index 8b4b5c400f..8ed6db6642 100644 --- a/apps/ardrone_interface/ardrone_interface.c +++ b/apps/ardrone_interface/ardrone_interface.c @@ -141,7 +141,6 @@ static int ardrone_open_uart(char *uart_name, struct termios *uart_config_origin int uart; /* open uart */ - //printf("[ardrone_interface] UART is %s, baud rate is%d\n",uart_name,speed); uart = open(uart_name, O_RDWR | O_NOCTTY); /* Try to set baud rate */ @@ -238,15 +237,14 @@ int ardrone_interface_thread_main(int argc, char *argv[]) /* declare and safely initialize all structs */ struct vehicle_status_s state; - //memset(&state, 0, sizeof(state)); + memset(&state, 0, sizeof(state)); struct actuator_controls_s actuator_controls; - //memset(&actuator_controls, 0, sizeof(actuator_controls)); + memset(&actuator_controls, 0, sizeof(actuator_controls)); struct actuator_armed_s armed; armed.armed = false; /* subscribe to attitude, motor setpoints and system state */ int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); - int state_sub = orb_subscribe(ORB_ID(vehicle_status)); int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); @@ -328,11 +326,6 @@ int ardrone_interface_thread_main(int argc, char *argv[]) * if in failsafe */ if (armed.armed && !armed.lockdown) { - - - - //printf("AMO_BEF: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",actuator_controls.control[0], actuator_controls.control[1], actuator_controls.control[2], actuator_controls.control[3]); - ardrone_mixing_and_output(ardrone_write, &actuator_controls); } else { diff --git a/apps/ardrone_interface/ardrone_motor_control.c b/apps/ardrone_interface/ardrone_motor_control.c index e410d3a71a..c68a26df96 100644 --- a/apps/ardrone_interface/ardrone_motor_control.c +++ b/apps/ardrone_interface/ardrone_motor_control.c @@ -388,16 +388,12 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls if (motor_thrust <= min_thrust) { motor_thrust = min_thrust; output_band = 0.0f; - //printf("0 silent\n"); } else if (motor_thrust < startpoint_full_control && motor_thrust > min_thrust) { output_band = band_factor * (motor_thrust - min_thrust); - //printf("1 starting\n"); } else if (motor_thrust >= startpoint_full_control && motor_thrust < max_thrust - band_factor * startpoint_full_control) { output_band = band_factor * startpoint_full_control; - //printf("2 working\n"); } else if (motor_thrust >= max_thrust - band_factor * startpoint_full_control) { output_band = band_factor * (max_thrust - motor_thrust); - //printf("3 full\n"); } //add the yaw, nick and roll components to the basic thrust //TODO:this should be done by the mixer diff --git a/apps/commander/commander.c b/apps/commander/commander.c index a1c2a15d26..1c23c1f9d2 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -404,10 +404,6 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) /* announce and set new offset */ - // char offset_output[50]; - // sprintf(offset_output, "[commander] mag cal: %8.4f %8.4f %8.4f", (double)mag_offset[0], (double)mag_offset[1], (double)mag_offset[2]); - // mavlink_log_info(mavlink_fd, offset_output); - if (param_set(param_find("SENS_MAG_XOFF"), &(mag_offset[0]))) { fprintf(stderr, "[commander] Setting X mag offset failed!\n"); } diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index b00b6bc0ca..ebd9911a36 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -236,7 +236,6 @@ mc_thread_main(int argc, char *argv[]) /* set yaw rate */ rates_sp.yaw = manual.yaw; att_sp.thrust = manual.throttle; - //printf("Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",att_sp.roll_body, att_sp.pitch_body, att_sp.yaw_body, att_sp.thrust); att_sp.timestamp = hrt_absolute_time(); /* STEP 2: publish the result to the vehicle actuators */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); @@ -278,15 +277,10 @@ mc_thread_main(int argc, char *argv[]) /* run attitude controller */ if (state.flag_control_attitude_enabled && !state.flag_control_rates_enabled) { multirotor_control_attitude(&att_sp, &att, NULL, &actuators); -// printf("publish actuator\n"); - -// printf("MAC_PUB: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",actuators.control[0], actuators.control[1], actuators.control[2], actuators.control[3]); - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); } else if (state.flag_control_attitude_enabled && state.flag_control_rates_enabled) { multirotor_control_attitude(&att_sp, &att, &rates_sp, NULL); orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); -// printf("publish attitude\n"); } diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c index c25e96856a..2129915d12 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -312,10 +312,6 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s actuators->control[3] = motor_thrust; } -// if(motor_skip_counter%20 == 0) -// printf("MAC: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",actuators->control[0], actuators->control[1], actuators->control[2], actuators->control[3]); - - // XXX change yaw rate to yaw pos controller if (rates_sp) { rates_sp->roll = roll_control; @@ -324,8 +320,5 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s rates_sp->thrust = motor_thrust; } -// if(motor_skip_counter%20 == 0) -// printf("Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",rates_sp->roll, rates_sp->pitch, rates_sp->yaw, rates_sp->thrust); - motor_skip_counter++; } diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index ceb8c4b104..f81dfa9b8f 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -1030,7 +1030,6 @@ Sensors::ppm_poll() orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc); orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control); -// printf("SENSORS: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",manual_control.roll, manual_control.pitch, manual_control.yaw, manual_control.throttle); } #endif From d206327541f159ac4abd76e66bce3160e8704231 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 27 Sep 2012 18:43:04 +0200 Subject: [PATCH 55/56] Magnetometer calibration fixed --- apps/commander/commander.c | 55 +++++++++++--------------------------- 1 file changed, 15 insertions(+), 40 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 1c23c1f9d2..960c5d3837 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -256,22 +256,6 @@ int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, u return 0; } -static void cal_bsort(float a[], int n) -{ - int i,j,t; - for(i=0;ia[j+1]) { - t=a[j]; - a[j]=a[j+1]; - a[j+1]=t; - } - } - } -} - static const char *parameter_file = "/eeprom/parameters"; static int pm_save_eeprom(bool only_unsaved) @@ -312,8 +296,8 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) const uint64_t calibration_interval_us = 45 * 1000000; unsigned int calibration_counter = 0; - float mag_max[3] = {0, 0, 0}; - float mag_min[3] = {0, 0, 0}; + float mag_max[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX}; + float mag_min[3] = {FLT_MAX, FLT_MAX, FLT_MAX}; int fd = open(MAG_DEVICE_PATH, 0); struct mag_scale mscale_null = { @@ -340,25 +324,25 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); /* get min/max values */ - if (raw.magnetometer_raw[0] < mag_min[0]) { - mag_min[0] = raw.magnetometer_raw[0]; + if (raw.magnetometer_ga[0] < mag_min[0]) { + mag_min[0] = raw.magnetometer_ga[0]; } - else if (raw.magnetometer_raw[0] > mag_max[0]) { - mag_max[0] = raw.magnetometer_raw[0]; + else if (raw.magnetometer_ga[0] > mag_max[0]) { + mag_max[0] = raw.magnetometer_ga[0]; } - if (raw.magnetometer_raw[1] < mag_min[1]) { - mag_min[1] = raw.magnetometer_raw[1]; + if (raw.magnetometer_ga[1] < mag_min[1]) { + mag_min[1] = raw.magnetometer_ga[1]; } - else if (raw.magnetometer_raw[1] > mag_max[1]) { - mag_max[1] = raw.magnetometer_raw[1]; + else if (raw.magnetometer_ga[1] > mag_max[1]) { + mag_max[1] = raw.magnetometer_ga[1]; } - if (raw.magnetometer_raw[2] < mag_min[2]) { - mag_min[2] = raw.magnetometer_raw[2]; + if (raw.magnetometer_ga[2] < mag_min[2]) { + mag_min[2] = raw.magnetometer_ga[2]; } - else if (raw.magnetometer_raw[2] > mag_max[2]) { - mag_max[2] = raw.magnetometer_raw[2]; + else if (raw.magnetometer_ga[2] > mag_max[2]) { + mag_max[2] = raw.magnetometer_ga[2]; } calibration_counter++; @@ -387,20 +371,11 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) * offset = (max + min) / 2.0f */ - printf("max 0: %f\n",mag_max[0]); - printf("max 1: %f\n",mag_max[1]); - printf("max 2: %f\n",mag_max[2]); - printf("min 0: %f\n",mag_min[0]); - printf("min 1: %f\n",mag_min[1]); - printf("min 2: %f\n",mag_min[2]); - mag_offset[0] = (mag_max[0] + mag_min[0]) / 2.0f; mag_offset[1] = (mag_max[1] + mag_min[1]) / 2.0f; mag_offset[2] = (mag_max[2] + mag_min[2]) / 2.0f; - printf("mag off 0: %f\n",mag_offset[0]); - printf("mag off 1: %f\n",mag_offset[1]); - printf("mag off 2: %f\n",mag_offset[2]); + printf("mag off x: %4.4f, y: %4.4f, z: %4.4f\n",(double)mag_offset[0],(double)mag_offset[0],(double)mag_offset[2]); /* announce and set new offset */ From fb691c9ff196599fbc12a776a1ea94ffc14967d4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 28 Sep 2012 16:28:51 +0200 Subject: [PATCH 56/56] Fix a bug where under really adverse conditions the system id is not read before the first heartbeat is send out, resulting in an immediately timing out system in the GCS --- apps/mavlink/mavlink.c | 57 ++++++++++++++++++++++++++---------------- 1 file changed, 36 insertions(+), 21 deletions(-) diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index bf0ed346ec..a1458ca03b 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -203,6 +203,7 @@ int mavlink_missionlib_send_gcs_string(const char *string); uint64_t mavlink_missionlib_get_system_timestamp(void); void handleMessage(mavlink_message_t *msg); +static void mavlink_update_system(); /** * Enable / disable Hardware in the Loop simulation mode. @@ -1548,6 +1549,39 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf return uart; } +void mavlink_update_system() +{ + static initialized = false; + param_t param_system_id; + param_t param_component_id; + param_t param_system_type; + + if (!initialized) { + param_system_id = param_find("MAV_SYS_ID"); + param_component_id = param_find("MAV_COMP_ID"); + param_system_type = param_find("MAV_TYPE"); + } + + /* update system and component id */ + int32_t system_id; + param_get(param_system_id, &system_id); + if (system_id > 0 && system_id < 255) { + mavlink_system.sysid = system_id; + } + + int32_t component_id; + param_get(param_component_id, &component_id); + if (component_id > 0 && component_id < 255) { + mavlink_system.compid = component_id; + } + + int32_t system_type; + param_get(param_system_type, &system_type); + if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) { + mavlink_system.type = system_type; + } +} + /** * MAVLink Protocol main function. */ @@ -1624,9 +1658,7 @@ int mavlink_thread_main(int argc, char *argv[]) fflush(stdout); /* Initialize system properties */ - param_t param_system_id = param_find("MAV_SYS_ID"); - param_t param_component_id = param_find("MAV_COMP_ID"); - param_t param_system_type = param_find("MAV_TYPE"); + mavlink_update_system(); /* topics to subscribe globally */ /* subscribe to ORB for global position */ @@ -1730,24 +1762,7 @@ int mavlink_thread_main(int argc, char *argv[]) /* 1 Hz */ if (lowspeed_counter == 10) { - /* update system and component id */ - int32_t system_id; - param_get(param_system_id, &system_id); - if (system_id > 0 && system_id < 255) { - mavlink_system.sysid = system_id; - } - - int32_t component_id; - param_get(param_component_id, &component_id); - if (component_id > 0 && component_id < 255) { - mavlink_system.compid = component_id; - } - - int32_t system_type; - param_get(param_system_type, &system_type); - if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) { - mavlink_system.type = system_type; - } + mavlink_update_system(); /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0;