From 291f4f3a33e6428b23624b1ffe12fec1015816cd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 19 Sep 2012 18:53:29 +0200 Subject: [PATCH 01/12] 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/12] 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)8c`y_X^k274?jy&t>Kl@)XkB4RlnP;q+~ml$?j^^v$Wg7xl{-YqRY&&qmVRP+9*|pa0ujV)N`}_B1-n=*O`n;L<-psQ!f!#2<*!)7H3OP_G-ys9xO?in{}i$CtXN#1=O=j_k!o6nh0>$HxW2J+wCpdB(F9 z4)jbUqwiTqi;^O0X6IxhkGivZ#Uq_w;`vKGu*D4<>8{AYS#o-gZ{n8s{_RY&c7j|~ ziYr_6mEffphu^gYgarg_wx_E8&~G0bbQ!T zk5`S5Yq>LAGS)Iw%JiRI&fG#b+idjr=F>V(v8EI84qo4`-EeM@2;nguR*{Irb!cv+ zm9@yUJzXM5Q4Uq25{ynXo{9O);qa={cPidJVzhWQ|LpBK&zV3S+Iz&K_eX;4s(k@1 zI!$*L*C7ef<1g)}`Qr|xR&)g$&8Na*^VWlBRM&QgrL)7Ut{_}SAs{)U&RfAS7yMMI zJ^t(!<3@j2P_IjED;%Ny_C`s#Vx z8tE7nIWJO-$PiFrVd}?l3aG(8Cu5*gd{jHDO05LlXo^=UetVp+79vnBl9AQ&)z)s^ zW{#!{?(5IcRa`a%tSBomQ6ga5<(YgMzd~ZVbm<2rwQ^XfRBt6wv)q=V%wFNpv>Rjd z{?m#A3YkP)VLrgAaJo<5UIm|~X>q!b<$OZs6TynKUA@}fY#~N9Crh%gutGK;SxT&s zrzrt@7^?LQJ5E+)ygl@m-|{Q^K~A>$6cSB=UZ<=BnR+OeVjuDWu|kP_llO(=6GMg;=;SEsnG2DzO6? zpNS<4(KM3@3Y0j`#0o4}=$$!{i+u*zb(P4ST*%vXgQ&t8B0@7|cUPbsOTGnMI7N|8 zo&m-`!UnS=co!5X+{w)fEhCqj1gPUQpoW z!%o%uZhw}c+;#NHZ{hAjvCDC@Vm}~2>0e#tnPJcLBs%t#^+DGSDpp$^4dSv}jtC!b z>R6J{D62I?ehf#LD#0=v7i@zQRXrF}+Kha>-F#p%1m|W5Bokf{5t0nI5z0mOggVDv zSE3kAmaErWDzVv=aF=qCKM~hQzI>lxLAuD1=x2P$O)LBK*ZSv4VwE9ZVgwPPJph+u zj3pCFh*Sw$**K*WkkA_1TSPFUbmo0WNYU4@Z~;bB5@G6iZ^tvTDy@tQ>O!5-WJpy0 z&0Fgu>(f?k8KGr890^_mKYHZLf9QtK6iI{e&$sz@4>{IYa@W_8ISPA7nO%1j zgM4SwTZ%qrYB-icaHVt>en)`m>PZAHN*o~Z_I-CYx;pAFIKHpB=)0fs(Y2esnzY8G zil^VHLJWt#X7)sTN~2k()axLv1 z8f^3&!FPTKeAf@5O{oQthuiya;Yvcq^MP4bY~BCb-)*e|Im zuj){n^r=!MB}BoDDm(GCQp-e%FcVsFC`TG}Q>UWO#=vHo?Pxl=wWbMr^$QKln%`sovT${6>QF@?yGtR0=oKp~8I)=o#Ivr^=)q0A=z;zY2Je)32=E6$9? zQ}B5cZ187|FT9a{9vJ8fK}SXA(RZzyi?nk^NTkB)L&E^cwRR z#ZQdc)_O7|bmp~Lj-0Pmn2Ejw?`^+&0tz*pBK0@X8v%7>3t9wkA zy7fWl2rC6V=6zch^0pU!N?i@&d=yPNP2w_jX8=G)9*t=yH3B7%`@mc0=K|=Sv%o9c z9#do{4*QYjvmldKKo z>!`B31x;GdhH8{Q%A8r0>!BWfc-zc}^84Yf`ysX2smi&^;EPz!Ku?87Al+y<&l9JX z2JU^c;8w6=7~19X@JjaLT1-UHVpnvxn@kTDAOJA$Cdzq$+oUCRke@7HX=%KO$bY3( z55bKc2omy(;23Vn{}|Kv5m;96s3?;(FmvyU4%IMiJ|j#9NwRC(q`z3r$|COC=CEqK z+_aOGKGlIn>PjD9v~=MaPi2Z<*dI@g&(a3J1YbEn-WGpNdN20I$2y6T^rkebf|&fn zq--P4mzx^j~c1HZC@H1v_h{`bRPQn~gIBZr?w&@o=%m8@8ch-XKH)Ndf0CDYN-a=CKg z(W`*13gE$HkdZC4^Iy4yM+c;$C*8ukmL&2suktm$1^h?IvkF{RY_03KRq)@i^?zY} z=$RY}h>e`JB0jt1px9-!Ewn8xDzkF5e}s0QKdf)2GTg`B(Q45JFhtJIT+!i10kbo=JTU;Pdf=p%nQ0*yA^ zPPXyhSW$nfz}rXvtBL#Com7y02}i4MTwt3w>+!BqPGbD0@F3I!80T zfK#inK6K&U$2%~`8goQ;?dIawF6YeHKh6iFmGp*Px|YJ2x7yfBs_q^fPIpgmxoD$B z16}aZ+A^2zsxgqu^*yd*qJ>I@+f33TWwtW!e~p{8mwIF)$K$FYic^Sz3+v} zR)5ce#8X;g9nIrIXWZcwi-KhKU?@52Ju7Z0Et>b6c# zdQOjHY=WMAEO zAG=lKDAs;zGXcyS@<6kdFZKP?LtjHAUY~$3bTKkc8v*L@n@F}kvY;u3?aWOq&tJ{3 zey6)(jg8CMA79XM?qRJ`F{V;R@;tCfVDkqa?bi>pw(sne4Bm^l9sfS{-jDgd?+sgp z-S~LM4kj-Id^gaRXUgpBMfd$wtX&z zvAyVOwZCC(lX%a@s|h7@^PTSS%=tmw7iGl{AFvbW&exuXLY0(zPbum_720{PN#y6@ zPUB!=`AMPiPT^@!GuEtbr9UxiCD3s+ z;(^ev&y&Yd@z8#!)2+X-U%C83h+Ec@g7dGfG~bKFdL59n3J6ugLh#Fx$2o>QGZE7X zaRT)kqF_xXl-xqN^8H8AZxsIq0ib`T%*TmU#mw zW0;Rvbw|L*MXYAP@@{GP%)8@CAO}evlS7dHpW&R3qc8KkRm7|#O$9cqYGGPZ`WfB@ zgXST6=x2|QVZx7B>7zW%qNa<&AY2!u$F=MhR0aihSx!#pQS?rosxr7cY@O+1q&vm=Tr{*Kg%GoclHz6eoR?!rL!P8>=!1sI+{Gxt3iNx|H(v=XuJv^sE?;(` zLnhRm*_y8PyPiJFfA$<#y!qKXg{l{>GtmFVH>xTr7WwA2T8EZ#j%5J19-XQDo%Z;H z@x;S;E{vJ;&o}Da?-(jkPHrVDw#+Eq|5)9+g&OORS+_qU;G_H3Z|U z4OB19?R6g|+>7g`hvkP&Ifq%rhVn@0oLx}MmMKVF`;q34r!;Hky9u(@SwzdO{c3I_ zx@V@w@Q2LpVSpV!S7WsxZ?nz+@32S!FKziXfR`0FuSeRhRI)DnwG@D8 v-Z+-@ldp=FG|OQD6_q%8c$MxXHRZ$+7c1oPblexCH(O*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/12] 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 082074f99196f8c936e21740a84b6738cb87875e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 21 Sep 2012 12:55:41 +0200 Subject: [PATCH 04/12] 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 05/12] 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 06/12] 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 07/12] 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 08/12] 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 a9b21886f32d0a8ef7cad8dfe7efbc3276f4fd58 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Sep 2012 18:44:01 +0200 Subject: [PATCH 09/12] 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 8b951ec417454353d61d19b3379e52b6da5dd6b6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Sep 2012 20:55:44 +0200 Subject: [PATCH 10/12] 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 de530d6ba1fcbcaf65fc78ac8cca3286fa52d624 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Sep 2012 01:20:41 +0200 Subject: [PATCH 11/12] 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 12/12] 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_ */