From 291f4f3a33e6428b23624b1ffe12fec1015816cd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 19 Sep 2012 18:53:29 +0200 Subject: [PATCH 01/24] 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/24] Updated EKF filter, untested --- apps/attitude_estimator_ekf/Makefile | 5 +- .../attitude_estimator_ekf_main.c | 62 +- .../codegen/attitudeKalmanfilter.c | 1031 +++++++++-------- .../codegen/attitudeKalmanfilter.h | 6 +- .../codegen/attitudeKalmanfilter_initialize.c | 2 +- .../codegen/attitudeKalmanfilter_initialize.h | 4 +- .../codegen/attitudeKalmanfilter_ref.rsp | 0 .../codegen/attitudeKalmanfilter_rtw.bat | 11 - .../codegen/attitudeKalmanfilter_rtw.mk | 328 ------ .../codegen/attitudeKalmanfilter_terminate.c | 2 +- .../codegen/attitudeKalmanfilter_terminate.h | 4 +- .../codegen/attitudeKalmanfilter_types.h | 2 +- .../codegen/buildInfo.mat | Bin 4114 -> 0 bytes apps/attitude_estimator_ekf/codegen/cross.c | 37 + apps/attitude_estimator_ekf/codegen/cross.h | 34 + apps/attitude_estimator_ekf/codegen/diag.c | 42 + apps/attitude_estimator_ekf/codegen/diag.h | 34 + apps/attitude_estimator_ekf/codegen/eye.c | 10 +- apps/attitude_estimator_ekf/codegen/eye.h | 6 +- apps/attitude_estimator_ekf/codegen/find.c | 77 ++ apps/attitude_estimator_ekf/codegen/find.h | 34 + .../attitude_estimator_ekf/codegen/mrdivide.c | 751 ++++++++++-- .../attitude_estimator_ekf/codegen/mrdivide.h | 6 +- apps/attitude_estimator_ekf/codegen/norm.c | 6 +- apps/attitude_estimator_ekf/codegen/norm.h | 4 +- .../attitude_estimator_ekf/codegen/rtGetInf.c | 2 +- .../attitude_estimator_ekf/codegen/rtGetInf.h | 2 +- .../attitude_estimator_ekf/codegen/rtGetNaN.c | 2 +- .../attitude_estimator_ekf/codegen/rtGetNaN.h | 2 +- .../codegen/rt_defines.h | 24 + .../codegen/rt_nonfinite.c | 2 +- .../codegen/rt_nonfinite.h | 2 +- .../codegen/rtw_proj.tmw | 1 - .../attitude_estimator_ekf/codegen/rtwtypes.h | 2 +- 34 files changed, 1595 insertions(+), 942 deletions(-) delete mode 100755 apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_ref.rsp delete mode 100755 apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.bat delete mode 100755 apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.mk delete mode 100755 apps/attitude_estimator_ekf/codegen/buildInfo.mat create mode 100755 apps/attitude_estimator_ekf/codegen/cross.c create mode 100755 apps/attitude_estimator_ekf/codegen/cross.h create mode 100755 apps/attitude_estimator_ekf/codegen/diag.c create mode 100755 apps/attitude_estimator_ekf/codegen/diag.h create mode 100755 apps/attitude_estimator_ekf/codegen/find.c create mode 100755 apps/attitude_estimator_ekf/codegen/find.h create mode 100755 apps/attitude_estimator_ekf/codegen/rt_defines.h delete mode 100755 apps/attitude_estimator_ekf/codegen/rtw_proj.tmw diff --git a/apps/attitude_estimator_ekf/Makefile b/apps/attitude_estimator_ekf/Makefile index cad20d3753..b4d62ed145 100644 --- a/apps/attitude_estimator_ekf/Makefile +++ b/apps/attitude_estimator_ekf/Makefile @@ -44,7 +44,10 @@ CSRCS = attitude_estimator_ekf_main.c \ codegen/rt_nonfinite.c \ codegen/rtGetInf.c \ codegen/rtGetNaN.c \ - codegen/norm.c + codegen/norm.c \ + codegen/find.c \ + codegen/diag.c \ + codegen/cross.c # XXX this is *horribly* broken INCLUDES += $(TOPDIR)/../mavlink/include/mavlink diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index 09cbdd4e94..6c18b044e5 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -80,21 +80,35 @@ static float dt = 1; /* 0, 0, -9.81, 1, 1, 1, wo (gyro offset), w */ /* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */ static float z_k[9] = {0}; /**< Measurement vector */ -static float x_aposteriori[12] = {0}; /**< */ -static float P_aposteriori[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f +static float x_aposteriori_k[9] = {0}; /**< */ +static float x_aposteriori[9] = {0}; +static float P_aposteriori_k[81] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 100.f, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 100.f, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 100.f, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 100.f, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 100.f, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 100.f, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 100.f, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 100.f, + 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, + }; +static float P_aposteriori[81] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 100.f, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 100.f, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 100.f, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 100.f, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 100.f, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 100.f, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 100.f, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 100.f, + 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, }; /**< init: diagonal matrix with big values */ -static float knownConst[7] = {1, 1, 1, 1, 1, 1, 1}; /**< knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */ +static float knownConst[20] = {1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}; /**< knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */ static float Rot_matrix[9] = {1.f, 0, 0, 0, 1.f, 0, 0, 0, 1.f @@ -203,13 +217,31 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) /* * function [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) */ + + //extern void attitudeKalmanfilter(real32_T dt, const int8_T updVect[9], const real32_T z_k_data[9], + // const int32_T z_k_sizes[1], const real32_T u[4], + // const real32_T x_aposteriori_k[9], const real32_T P_aposteriori_k[81], + // const real32_T knownConst[20], real32_T eulerAngles[3], + // real32_T Rot_matrix[9], real32_T x_aposteriori[9], + // real32_T P_aposteriori[81]); + + int8_t update_vect[9] = {1, 1, 1, 1, 1, 1, 1, 1, 1}; + float euler[3]; + int32_t z_k_sizes = 9; + float u[4] = {0.0f, 0.0f, 0.0f, 0.0f}; + uint64_t timing_start = hrt_absolute_time(); - attitudeKalmanfilter(dt, z_k, x_aposteriori, P_aposteriori, knownConst, Rot_matrix, x_aposteriori, P_aposteriori); + attitudeKalmanfilter(dt, update_vect, z_k, &z_k_sizes, u, x_aposteriori_k, P_aposteriori_k, knownConst, euler, + Rot_matrix, x_aposteriori, P_aposteriori); + /* swap values for next iteration */ + memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k)); + memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k)); uint64_t timing_diff = hrt_absolute_time() - timing_start; /* print rotation matrix every 200th time */ if (printcounter % 200 == 0) { printf("EKF attitude iteration: %d, runtime: %d us, dt: %d us (%d Hz)\n", loopcounter, (int)timing_diff, (int)(dt * 1000000.0f), (int)(1.0f / dt)); + printf("roll: %8.4f\tpitch: %8.4f\tyaw:%8.4f", euler[0], euler[1], euler[2]); printf("\n%d\t%d\t%d\n%d\t%d\t%d\n%d\t%d\t%d\n", (int)(Rot_matrix[0] * 100), (int)(Rot_matrix[1] * 100), (int)(Rot_matrix[2] * 100), (int)(Rot_matrix[3] * 100), (int)(Rot_matrix[4] * 100), (int)(Rot_matrix[5] * 100), (int)(Rot_matrix[6] * 100), (int)(Rot_matrix[7] * 100), (int)(Rot_matrix[8] * 100)); diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c index 4e275e3ee9..321e6874d0 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:22 2012 * */ @@ -11,8 +11,11 @@ #include "rt_nonfinite.h" #include "attitudeKalmanfilter.h" #include "norm.h" +#include "cross.h" #include "eye.h" #include "mrdivide.h" +#include "diag.h" +#include "find.h" /* Type Definitions */ @@ -23,613 +26,691 @@ /* Variable Definitions */ /* Function Declarations */ +static real32_T rt_atan2f_snf(real32_T u0, real32_T u1); /* Function Definitions */ +static real32_T rt_atan2f_snf(real32_T u0, real32_T u1) +{ + real32_T y; + if (rtIsNaNF(u0) || rtIsNaNF(u1)) { + y = ((real32_T)rtNaN); + } else if (rtIsInfF(u0) && rtIsInfF(u1)) { + y = (real32_T)atan2(u0 > 0.0F ? 1.0F : -1.0F, u1 > 0.0F ? 1.0F : -1.0F); + } else if (u1 == 0.0F) { + if (u0 > 0.0F) { + y = RT_PIF / 2.0F; + } else if (u0 < 0.0F) { + y = -(RT_PIF / 2.0F); + } else { + y = 0.0F; + } + } else { + y = (real32_T)atan2(u0, u1); + } + + return y; +} /* - * function [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) + * function [eulerAngles,Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,updVect,z_k,u,x_aposteriori_k,P_aposteriori_k,knownConst) */ -void attitudeKalmanfilter(real32_T dt, const real32_T z_k[9], const real32_T - x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T - knownConst[7], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T - P_aposteriori[144]) +void attitudeKalmanfilter(real32_T dt, const int8_T updVect[9], const real32_T + z_k_data[9], const int32_T z_k_sizes[1], const real32_T u[4], const real32_T + x_aposteriori_k[9], const real32_T P_aposteriori_k[81], const real32_T + knownConst[20], real32_T eulerAngles[3], real32_T Rot_matrix[9], real32_T + x_aposteriori[9], real32_T P_aposteriori[81]) { + int32_T udpIndVect_sizes; + real_T udpIndVect_data[9]; real32_T R_temp[9]; real_T dv0[9]; - real_T dv1[9]; - int32_T i; + int32_T ib; int32_T i0; - real32_T A_pred[144]; - real32_T x_apriori[12]; - real32_T b_A_pred[144]; + real32_T fv0[81]; + real32_T b_knownConst[27]; + real32_T fv1[9]; + real32_T c_knownConst[9]; + real_T dv1[9]; + real_T dv2[9]; + real32_T A_lin[81]; + real32_T x_n_b[3]; + real32_T K_k_data[81]; int32_T i1; - real32_T c_A_pred[144]; + real32_T b_A_lin[81]; static const int8_T iv0[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; - real32_T P_apriori[144]; - real32_T b_P_apriori[108]; - static const int8_T iv1[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, - 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1 }; - - real32_T K_k[108]; - static const int8_T iv2[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, + real32_T P_apriori[81]; + int32_T ia; + static const int8_T H_k_full[81] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; - real32_T fv0[81]; - real32_T fv1[81]; - real32_T fv2[81]; - real32_T B; - real_T dv2[144]; - real32_T b_B; - real32_T earth_z[3]; - real32_T y[3]; - real32_T earth_x[3]; + int8_T H_k_data[81]; + int32_T accUpt; + int32_T magUpt; + real32_T y; + real32_T y_k_data[9]; + int32_T P_apriori_sizes[2]; + int32_T H_k_sizes[2]; + int32_T K_k_sizes[2]; + real32_T b_y[9]; + real_T dv3[81]; + real32_T c_y; + real32_T z_n_b[3]; + real32_T y_n_b[3]; /* Extended Attitude Kalmanfilter */ - /* */ + /* */ /* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */ /* measurement vector z has the following entries [ax,ay,az||mx,my,mz||wmx,wmy,wmz]' */ /* knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */ /* */ /* [x_aposteriori,P_aposteriori] = AttKalman(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) */ /* */ - /* Example.... */ + /* Example.... */ /* */ /* $Author: Tobias Naegeli $ $Date: 2012 $ $Revision: 1 $ */ /* %define the matrices */ - /* 'attitudeKalmanfilter:19' acc_ProcessNoise=knownConst(1); */ - /* 'attitudeKalmanfilter:20' mag_ProcessNoise=knownConst(2); */ - /* 'attitudeKalmanfilter:21' ratesOffset_ProcessNoise=knownConst(3); */ - /* 'attitudeKalmanfilter:22' rates_ProcessNoise=knownConst(4); */ - /* 'attitudeKalmanfilter:25' acc_MeasurementNoise=knownConst(5); */ - /* 'attitudeKalmanfilter:26' mag_MeasurementNoise=knownConst(6); */ - /* 'attitudeKalmanfilter:27' rates_MeasurementNoise=knownConst(7); */ + /* tpred=0.005; */ + /* */ + /* A=[ 0, 0, 0, 0, 0, 0, 0, 0, 0; */ + /* 0, 0, 0, 0, 0, 0, 0, 0, 0; */ + /* 0, 0, 0, 0, 0, 0, 0, 0, 0; */ + /* 0, -1, 1, 0, 0, 0, 0, 0, 0; */ + /* 1, 0, -1, 0, 0, 0, 0, 0, 0; */ + /* -1, 1, 0, 0, 0, 0, 0, 0, 0; */ + /* 0, -1, 1, 0, 0, 0, 0, 0, 0; */ + /* 1, 0, -1, 0, 0, 0, 0, 0, 0; */ + /* -1, 1, 0, 0, 0, 0, 0, 0, 0]; */ + /* */ + /* */ + /* b=[70, 0, 0; */ + /* 0, 70, 0; */ + /* 0, 0, 0; */ + /* 0, 0, 0; */ + /* 0, 0, 0; */ + /* 0, 0, 0; */ + /* 0, 0, 0; */ + /* 0, 0, 0; */ + /* 0, 0, 0]; */ + /* */ + /* */ + /* C=[1, 0, 0, 0, 0, 0, 0, 0, 0; */ + /* 0, 1, 0, 0, 0, 0, 0, 0, 0; */ + /* 0, 0, 1, 0, 0, 0, 0, 0, 0; */ + /* 0, 0, 0, 1, 0, 0, 0, 0, 0; */ + /* 0, 0, 0, 0, 1, 0, 0, 0, 0; */ + /* 0, 0, 0, 0, 0, 1, 0, 0, 0; */ + /* 0, 0, 0, 0, 0, 0, 1, 0, 0; */ + /* 0, 0, 0, 0, 0, 0, 0, 1, 0; */ + /* 0, 0, 0, 0, 0, 0, 0, 0, 1]; */ + /* D=[]; */ + /* */ + /* */ + /* sys=ss(A,b,C,D); */ + /* */ + /* sysd=c2d(sys,tpred); */ + /* */ + /* */ + /* F=sysd.a; */ + /* */ + /* B=sysd.b; */ + /* */ + /* H=sysd.c; */ + /* 'attitudeKalmanfilter:68' udpIndVect=find(updVect); */ + find(updVect, udpIndVect_data, &udpIndVect_sizes); + + /* 'attitudeKalmanfilter:71' rates_ProcessNoiseMatrix=diag([knownConst(1),knownConst(1),knownConst(2)]); */ + /* 'attitudeKalmanfilter:72' acc_ProcessNoise=knownConst(3); */ + /* 'attitudeKalmanfilter:73' mag_ProcessNoise=knownConst(4); */ + /* 'attitudeKalmanfilter:74' rates_MeasurementNoise=knownConst(6); */ + /* 'attitudeKalmanfilter:75' acc_MeasurementNoise=knownConst(7); */ + /* 'attitudeKalmanfilter:76' mag_MeasurementNoise=knownConst(8); */ /* process noise covariance matrix */ - /* 'attitudeKalmanfilter:30' Q = [ eye(3)*acc_ProcessNoise, zeros(3), zeros(3), zeros(3); */ - /* 'attitudeKalmanfilter:31' zeros(3), eye(3)*mag_ProcessNoise, zeros(3), zeros(3); */ - /* 'attitudeKalmanfilter:32' zeros(3), zeros(3), eye(3)*ratesOffset_ProcessNoise, zeros(3); */ - /* 'attitudeKalmanfilter:33' zeros(3), zeros(3), zeros(3), eye(3)*rates_ProcessNoise]; */ - /* measurement noise covariance matrix */ - /* 'attitudeKalmanfilter:36' R = [ eye(3)*acc_MeasurementNoise, zeros(3), zeros(3); */ - /* 'attitudeKalmanfilter:37' zeros(3), eye(3)*mag_MeasurementNoise, zeros(3); */ - /* 'attitudeKalmanfilter:38' zeros(3), zeros(3), eye(3)*rates_MeasurementNoise]; */ + /* 'attitudeKalmanfilter:81' Q = [ rates_ProcessNoiseMatrix, zeros(3), zeros(3); */ + /* 'attitudeKalmanfilter:82' zeros(3), eye(3)*mag_ProcessNoise, zeros(3); */ + /* 'attitudeKalmanfilter:83' zeros(3), zeros(3), eye(3)*acc_ProcessNoise]; */ /* observation matrix */ - /* 'attitudeKalmanfilter:42' H_k=[ eye(3), zeros(3), zeros(3), zeros(3); */ - /* 'attitudeKalmanfilter:43' zeros(3), eye(3), zeros(3), zeros(3); */ - /* 'attitudeKalmanfilter:44' zeros(3), zeros(3), eye(3), eye(3)]; */ + /* 'attitudeKalmanfilter:89' H_k_full=[ eye(3), zeros(3), zeros(3); */ + /* 'attitudeKalmanfilter:90' zeros(3), eye(3), zeros(3); */ + /* 'attitudeKalmanfilter:91' zeros(3), zeros(3), eye(3)]; */ /* compute A(t,w) */ /* x_aposteriori_k[10,11,12] should be [p,q,r] */ /* R_temp=[1,-r, q */ /* r, 1, -p */ /* -q, p, 1] */ - /* 'attitudeKalmanfilter:53' R_temp=[1,-dt*x_aposteriori_k(12),dt*x_aposteriori_k(11); */ - /* 'attitudeKalmanfilter:54' dt*x_aposteriori_k(12),1,-dt*x_aposteriori_k(10); */ - /* 'attitudeKalmanfilter:55' -dt*x_aposteriori_k(11), dt*x_aposteriori_k(10),1]; */ + /* 'attitudeKalmanfilter:100' R_temp=[1, -dt*x_aposteriori_k(3), dt*x_aposteriori_k(2); */ + /* 'attitudeKalmanfilter:101' dt*x_aposteriori_k(3), 1, -dt*x_aposteriori_k(1); */ + /* 'attitudeKalmanfilter:102' -dt*x_aposteriori_k(2), dt*x_aposteriori_k(1), 1]; */ R_temp[0] = 1.0F; - R_temp[3] = -dt * x_aposteriori_k[11]; - R_temp[6] = dt * x_aposteriori_k[10]; - R_temp[1] = dt * x_aposteriori_k[11]; + R_temp[3] = -dt * x_aposteriori_k[2]; + R_temp[6] = dt * x_aposteriori_k[1]; + R_temp[1] = dt * x_aposteriori_k[2]; R_temp[4] = 1.0F; - R_temp[7] = -dt * x_aposteriori_k[9]; - R_temp[2] = -dt * x_aposteriori_k[10]; - R_temp[5] = dt * x_aposteriori_k[9]; + R_temp[7] = -dt * x_aposteriori_k[0]; + R_temp[2] = -dt * x_aposteriori_k[1]; + R_temp[5] = dt * x_aposteriori_k[0]; R_temp[8] = 1.0F; - /* strange, should not be transposed */ - /* 'attitudeKalmanfilter:58' A_pred=[R_temp', zeros(3), zeros(3), zeros(3); */ - /* 'attitudeKalmanfilter:59' zeros(3), R_temp', zeros(3), zeros(3); */ - /* 'attitudeKalmanfilter:60' zeros(3), zeros(3), eye(3), zeros(3); */ - /* 'attitudeKalmanfilter:61' zeros(3), zeros(3), zeros(3), eye(3)]; */ - eye(dv0); - eye(dv1); - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[i0 + 12 * i] = R_temp[i + 3 * i0]; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[i0 + 12 * (i + 3)] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[i0 + 12 * (i + 6)] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[i0 + 12 * (i + 9)] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * i) + 3] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 3)) + 3] = R_temp[i + 3 * i0]; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 6)) + 3] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 9)) + 3] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * i) + 6] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 3)) + 6] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 6)) + 6] = (real32_T)dv0[i0 + 3 * i]; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 9)) + 6] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * i) + 9] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 3)) + 9] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 6)) + 9] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 9)) + 9] = (real32_T)dv1[i0 + 3 * i]; - } - } - + /* 'attitudeKalmanfilter:106' A_pred=[eye(3), zeros(3), zeros(3); */ + /* 'attitudeKalmanfilter:107' zeros(3), R_temp', zeros(3); */ + /* 'attitudeKalmanfilter:108' zeros(3), zeros(3), R_temp']; */ + /* 'attitudeKalmanfilter:110' B_pred=[knownConst(9)*dt, 0, 0; */ + /* 'attitudeKalmanfilter:111' 0, knownConst(10)*dt, 0; */ + /* 'attitudeKalmanfilter:112' 0, 0, 0; */ + /* 'attitudeKalmanfilter:113' 0, 0, 0; */ + /* 'attitudeKalmanfilter:114' 0, 0, 0; */ + /* 'attitudeKalmanfilter:115' 0, 0, 0; */ + /* 'attitudeKalmanfilter:116' 0, 0, 0; */ + /* 'attitudeKalmanfilter:117' 0, 0, 0; */ + /* 'attitudeKalmanfilter:118' 0, 0, 0]; */ /* %prediction step */ - /* 'attitudeKalmanfilter:64' x_apriori=A_pred*x_aposteriori_k; */ - for (i = 0; i < 12; i++) { - x_apriori[i] = 0.0F; - for (i0 = 0; i0 < 12; i0++) { - x_apriori[i] += A_pred[i + 12 * i0] * x_aposteriori_k[i0]; + /* 'attitudeKalmanfilter:121' x_apriori=A_pred*x_aposteriori_k+B_pred*u(1:3); */ + eye(dv0); + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[i0 + 9 * ib] = (real32_T)dv0[i0 + 3 * ib]; } } + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[i0 + 9 * (ib + 3)] = 0.0F; + } + } + + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[i0 + 9 * (ib + 6)] = 0.0F; + } + } + + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[(i0 + 9 * ib) + 3] = 0.0F; + } + } + + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[(i0 + 9 * (ib + 3)) + 3] = R_temp[ib + 3 * i0]; + } + } + + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[(i0 + 9 * (ib + 6)) + 3] = 0.0F; + } + } + + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[(i0 + 9 * ib) + 6] = 0.0F; + } + } + + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[(i0 + 9 * (ib + 3)) + 6] = 0.0F; + } + } + + b_knownConst[0] = knownConst[8] * dt; + b_knownConst[9] = 0.0F; + b_knownConst[18] = 0.0F; + b_knownConst[1] = 0.0F; + b_knownConst[10] = knownConst[9] * dt; + b_knownConst[19] = 0.0F; + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[(i0 + 9 * (ib + 6)) + 6] = R_temp[ib + 3 * i0]; + } + + b_knownConst[2 + 9 * ib] = 0.0F; + b_knownConst[3 + 9 * ib] = 0.0F; + b_knownConst[4 + 9 * ib] = 0.0F; + b_knownConst[5 + 9 * ib] = 0.0F; + b_knownConst[6 + 9 * ib] = 0.0F; + b_knownConst[7 + 9 * ib] = 0.0F; + b_knownConst[8 + 9 * ib] = 0.0F; + } + + for (ib = 0; ib < 9; ib++) { + fv1[ib] = 0.0F; + for (i0 = 0; i0 < 9; i0++) { + fv1[ib] += fv0[ib + 9 * i0] * x_aposteriori_k[i0]; + } + + c_knownConst[ib] = 0.0F; + for (i0 = 0; i0 < 3; i0++) { + c_knownConst[ib] += b_knownConst[ib + 9 * i0] * u[i0]; + } + + x_aposteriori[ib] = fv1[ib] + c_knownConst[ib]; + } + /* linearization */ - /* 'attitudeKalmanfilter:67' acc_temp_mat=[0, dt*x_aposteriori_k(3), -dt*x_aposteriori_k(2); */ - /* 'attitudeKalmanfilter:68' -dt*x_aposteriori_k(3), 0, dt*x_aposteriori_k(1); */ - /* 'attitudeKalmanfilter:69' dt*x_aposteriori_k(2), -dt*x_aposteriori_k(1), 0]; */ - /* 'attitudeKalmanfilter:71' mag_temp_mat=[0, dt*x_aposteriori_k(6), -dt*x_aposteriori_k(5); */ - /* 'attitudeKalmanfilter:72' -dt*x_aposteriori_k(6), 0, dt*x_aposteriori_k(4); */ - /* 'attitudeKalmanfilter:73' dt*x_aposteriori_k(5), -dt*x_aposteriori_k(4), 0]; */ - /* 'attitudeKalmanfilter:75' A_lin=[R_temp', zeros(3), zeros(3), acc_temp_mat'; */ - /* 'attitudeKalmanfilter:76' zeros(3), R_temp', zeros(3), mag_temp_mat'; */ - /* 'attitudeKalmanfilter:77' zeros(3), zeros(3), eye(3), zeros(3); */ - /* 'attitudeKalmanfilter:78' zeros(3), zeros(3), zeros(3), eye(3)]; */ + /* 'attitudeKalmanfilter:125' temp_mat=[ 0, -dt, dt; */ + /* 'attitudeKalmanfilter:126' dt, 0, -dt; */ + /* 'attitudeKalmanfilter:127' -dt, dt, 0]; */ + R_temp[0] = 0.0F; + R_temp[3] = -dt; + R_temp[6] = dt; + R_temp[1] = dt; + R_temp[4] = 0.0F; + R_temp[7] = -dt; + R_temp[2] = -dt; + R_temp[5] = dt; + R_temp[8] = 0.0F; + + /* 'attitudeKalmanfilter:129' A_lin=[ eye(3), zeros(3), zeros(3); */ + /* 'attitudeKalmanfilter:130' temp_mat, eye(3) , zeros(3); */ + /* 'attitudeKalmanfilter:131' temp_mat, zeros(3), eye(3)]; */ eye(dv0); eye(dv1); - for (i = 0; i < 3; i++) { + eye(dv2); + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - A_pred[i0 + 12 * i] = R_temp[i + 3 * i0]; + A_lin[i0 + 9 * ib] = (real32_T)dv0[i0 + 3 * ib]; } } - for (i = 0; i < 3; i++) { + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - A_pred[i0 + 12 * (i + 3)] = 0.0F; + A_lin[i0 + 9 * (ib + 3)] = 0.0F; } } - for (i = 0; i < 3; i++) { + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - A_pred[i0 + 12 * (i + 6)] = 0.0F; + A_lin[i0 + 9 * (ib + 6)] = 0.0F; } } - A_pred[108] = 0.0F; - A_pred[109] = dt * x_aposteriori_k[2]; - A_pred[110] = -dt * x_aposteriori_k[1]; - A_pred[120] = -dt * x_aposteriori_k[2]; - A_pred[121] = 0.0F; - A_pred[122] = dt * x_aposteriori_k[0]; - A_pred[132] = dt * x_aposteriori_k[1]; - A_pred[133] = -dt * x_aposteriori_k[0]; - A_pred[134] = 0.0F; - for (i = 0; i < 3; i++) { + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * i) + 3] = 0.0F; + A_lin[(i0 + 9 * ib) + 3] = R_temp[i0 + 3 * ib]; } } - for (i = 0; i < 3; i++) { + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 3)) + 3] = R_temp[i + 3 * i0]; + A_lin[(i0 + 9 * (ib + 3)) + 3] = (real32_T)dv1[i0 + 3 * ib]; } } - for (i = 0; i < 3; i++) { + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 6)) + 3] = 0.0F; + A_lin[(i0 + 9 * (ib + 6)) + 3] = 0.0F; } } - A_pred[111] = 0.0F; - A_pred[112] = dt * x_aposteriori_k[5]; - A_pred[113] = -dt * x_aposteriori_k[4]; - A_pred[123] = -dt * x_aposteriori_k[5]; - A_pred[124] = 0.0F; - A_pred[125] = dt * x_aposteriori_k[3]; - A_pred[135] = dt * x_aposteriori_k[4]; - A_pred[136] = -dt * x_aposteriori_k[3]; - A_pred[137] = 0.0F; - for (i = 0; i < 3; i++) { + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * i) + 6] = 0.0F; + A_lin[(i0 + 9 * ib) + 6] = R_temp[i0 + 3 * ib]; } } - for (i = 0; i < 3; i++) { + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 3)) + 6] = 0.0F; + A_lin[(i0 + 9 * (ib + 3)) + 6] = 0.0F; } } - for (i = 0; i < 3; i++) { + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 6)) + 6] = (real32_T)dv0[i0 + 3 * i]; + A_lin[(i0 + 9 * (ib + 6)) + 6] = (real32_T)dv2[i0 + 3 * ib]; } } - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 9)) + 6] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * i) + 9] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 3)) + 9] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 6)) + 9] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 9)) + 9] = (real32_T)dv1[i0 + 3 * i]; - } - } - - /* 'attitudeKalmanfilter:81' P_apriori=A_lin*P_aposteriori_k*A_lin'+Q; */ - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - b_A_pred[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - b_A_pred[i + 12 * i0] += A_pred[i + 12 * i1] * P_aposteriori_k[i1 + 12 * + /* 'attitudeKalmanfilter:134' P_apriori=A_lin*P_aposteriori_k*A_lin'+Q; */ + x_n_b[0] = knownConst[0]; + x_n_b[1] = knownConst[0]; + x_n_b[2] = knownConst[1]; + diag(x_n_b, R_temp); + for (ib = 0; ib < 9; ib++) { + for (i0 = 0; i0 < 9; i0++) { + K_k_data[ib + 9 * i0] = 0.0F; + for (i1 = 0; i1 < 9; i1++) { + K_k_data[ib + 9 * i0] += A_lin[ib + 9 * i1] * P_aposteriori_k[i1 + 9 * i0]; } } - for (i0 = 0; i0 < 12; i0++) { - c_A_pred[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - c_A_pred[i + 12 * i0] += b_A_pred[i + 12 * i1] * A_pred[i0 + 12 * i1]; + for (i0 = 0; i0 < 9; i0++) { + b_A_lin[ib + 9 * i0] = 0.0F; + for (i1 = 0; i1 < 9; i1++) { + b_A_lin[ib + 9 * i0] += K_k_data[ib + 9 * i1] * A_lin[i0 + 9 * i1]; } } } - for (i = 0; i < 3; i++) { + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - b_A_pred[i0 + 12 * i] = (real32_T)iv0[i0 + 3 * i] * knownConst[0]; + K_k_data[i0 + 9 * ib] = R_temp[i0 + 3 * ib]; } } - for (i = 0; i < 3; i++) { + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - b_A_pred[i0 + 12 * (i + 3)] = 0.0F; + K_k_data[i0 + 9 * (ib + 3)] = 0.0F; } } - for (i = 0; i < 3; i++) { + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - b_A_pred[i0 + 12 * (i + 6)] = 0.0F; + K_k_data[i0 + 9 * (ib + 6)] = 0.0F; } } - for (i = 0; i < 3; i++) { + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - b_A_pred[i0 + 12 * (i + 9)] = 0.0F; + K_k_data[(i0 + 9 * ib) + 3] = 0.0F; } } - for (i = 0; i < 3; i++) { + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - b_A_pred[(i0 + 12 * i) + 3] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - b_A_pred[(i0 + 12 * (i + 3)) + 3] = (real32_T)iv0[i0 + 3 * i] * - knownConst[1]; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - b_A_pred[(i0 + 12 * (i + 6)) + 3] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - b_A_pred[(i0 + 12 * (i + 9)) + 3] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - b_A_pred[(i0 + 12 * i) + 6] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - b_A_pred[(i0 + 12 * (i + 3)) + 6] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - b_A_pred[(i0 + 12 * (i + 6)) + 6] = (real32_T)iv0[i0 + 3 * i] * - knownConst[2]; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - b_A_pred[(i0 + 12 * (i + 9)) + 6] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - b_A_pred[(i0 + 12 * i) + 9] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - b_A_pred[(i0 + 12 * (i + 3)) + 9] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - b_A_pred[(i0 + 12 * (i + 6)) + 9] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - b_A_pred[(i0 + 12 * (i + 9)) + 9] = (real32_T)iv0[i0 + 3 * i] * + K_k_data[(i0 + 9 * (ib + 3)) + 3] = (real32_T)iv0[i0 + 3 * ib] * knownConst[3]; } } - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - P_apriori[i0 + 12 * i] = c_A_pred[i0 + 12 * i] + b_A_pred[i0 + 12 * i]; + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + K_k_data[(i0 + 9 * (ib + 6)) + 3] = 0.0F; } } - /* %update step */ - /* 'attitudeKalmanfilter:86' y_k=z_k-H_k*x_apriori; */ - /* 'attitudeKalmanfilter:87' S_k=H_k*P_apriori*H_k'+R; */ - /* 'attitudeKalmanfilter:88' K_k=(P_apriori*H_k'/(S_k)); */ - for (i = 0; i < 12; i++) { + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + K_k_data[(i0 + 9 * ib) + 6] = 0.0F; + } + } + + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + K_k_data[(i0 + 9 * (ib + 3)) + 6] = 0.0F; + } + } + + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + K_k_data[(i0 + 9 * (ib + 6)) + 6] = (real32_T)iv0[i0 + 3 * ib] * + knownConst[2]; + } + } + + for (ib = 0; ib < 9; ib++) { for (i0 = 0; i0 < 9; i0++) { - b_P_apriori[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - b_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)iv1[i1 + - 12 * i0]; - } + P_apriori[i0 + 9 * ib] = b_A_lin[i0 + 9 * ib] + K_k_data[i0 + 9 * ib]; } } - for (i = 0; i < 9; i++) { - for (i0 = 0; i0 < 12; i0++) { - K_k[i + 9 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - K_k[i + 9 * i0] += (real32_T)iv2[i + 9 * i1] * P_apriori[i1 + 12 * i0]; + /* 'attitudeKalmanfilter:137' if ~isempty(udpIndVect)==1 */ + if (!(udpIndVect_sizes == 0) == 1) { + /* 'attitudeKalmanfilter:138' H_k= H_k_full(udpIndVect,:); */ + for (ib = 0; ib < 9; ib++) { + ia = udpIndVect_sizes - 1; + for (i0 = 0; i0 <= ia; i0++) { + H_k_data[i0 + udpIndVect_sizes * ib] = H_k_full[((int32_T) + udpIndVect_data[i0] + 9 * ib) - 1]; } } - for (i0 = 0; i0 < 9; i0++) { - fv0[i + 9 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - fv0[i + 9 * i0] += K_k[i + 9 * i1] * (real32_T)iv1[i1 + 12 * i0]; - } - } - } + /* %update step */ + /* 'attitudeKalmanfilter:140' accUpt=1; */ + accUpt = 1; - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - fv1[i0 + 9 * i] = (real32_T)iv0[i0 + 3 * i] * knownConst[4]; - } - } + /* 'attitudeKalmanfilter:141' magUpt=1; */ + magUpt = 1; - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - fv1[i0 + 9 * (i + 3)] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - fv1[i0 + 9 * (i + 6)] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - fv1[(i0 + 9 * i) + 3] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - fv1[(i0 + 9 * (i + 3)) + 3] = (real32_T)iv0[i0 + 3 * i] * knownConst[5]; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - fv1[(i0 + 9 * (i + 6)) + 3] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - fv1[(i0 + 9 * i) + 6] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - fv1[(i0 + 9 * (i + 3)) + 6] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - fv1[(i0 + 9 * (i + 6)) + 6] = (real32_T)iv0[i0 + 3 * i] * knownConst[6]; - } - } - - for (i = 0; i < 9; i++) { - for (i0 = 0; i0 < 9; i0++) { - fv2[i0 + 9 * i] = fv0[i0 + 9 * i] + fv1[i0 + 9 * i]; - } - } - - mrdivide(b_P_apriori, fv2, K_k); - - /* 'attitudeKalmanfilter:91' x_aposteriori=x_apriori+K_k*y_k; */ - for (i = 0; i < 9; i++) { - B = 0.0F; - for (i0 = 0; i0 < 12; i0++) { - B += (real32_T)iv2[i + 9 * i0] * x_apriori[i0]; - } - - R_temp[i] = z_k[i] - B; - } - - for (i = 0; i < 12; i++) { - B = 0.0F; - for (i0 = 0; i0 < 9; i0++) { - B += K_k[i + 12 * i0] * R_temp[i0]; - } - - x_aposteriori[i] = x_apriori[i] + B; - } - - /* 'attitudeKalmanfilter:92' P_aposteriori=(eye(12)-K_k*H_k)*P_apriori; */ - b_eye(dv2); - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - B = 0.0F; - for (i1 = 0; i1 < 9; i1++) { - B += K_k[i + 12 * i1] * (real32_T)iv2[i1 + 9 * i0]; + /* 'attitudeKalmanfilter:142' y_k=z_k-H_k*x_apriori; */ + ia = udpIndVect_sizes - 1; + for (ib = 0; ib <= ia; ib++) { + y = 0.0F; + for (i0 = 0; i0 < 9; i0++) { + y += (real32_T)H_k_data[ib + udpIndVect_sizes * i0] * x_aposteriori[i0]; } - A_pred[i + 12 * i0] = (real32_T)dv2[i + 12 * i0] - B; + y_k_data[ib] = z_k_data[ib] - y; } - } - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - P_aposteriori[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - P_aposteriori[i + 12 * i0] += A_pred[i + 12 * i1] * P_apriori[i1 + 12 * - i0]; + /* 'attitudeKalmanfilter:143' if updVect(4)==1 */ + if (updVect[3] == 1) { + /* 'attitudeKalmanfilter:144' if (abs(norm(z_k(4:6))-knownConst(12))>knownConst(14)) */ + for (ib = 0; ib < 3; ib++) { + x_n_b[ib] = z_k_data[ib + 3]; + } + + if ((real32_T)fabs(norm(x_n_b) - knownConst[11]) > knownConst[13]) { + /* 'attitudeKalmanfilter:145' accUpt=10000; */ + accUpt = 10000; } } + + /* 'attitudeKalmanfilter:149' if updVect(7)==1 */ + if (updVect[6] == 1) { + /* 'attitudeKalmanfilter:150' if (abs(norm(z_k(7:9))-knownConst(13))>knownConst(15)) */ + for (ib = 0; ib < 3; ib++) { + x_n_b[ib] = z_k_data[ib + 6]; + } + + if ((real32_T)fabs(norm(x_n_b) - knownConst[12]) > knownConst[14]) { + /* 'attitudeKalmanfilter:152' magUpt=10000; */ + magUpt = 10000; + } + } + + /* measurement noise covariance matrix */ + /* 'attitudeKalmanfilter:157' R = [ eye(3)*rates_MeasurementNoise, zeros(3), zeros(3); */ + /* 'attitudeKalmanfilter:158' zeros(3), eye(3)*acc_MeasurementNoise*accUpt, zeros(3); */ + /* 'attitudeKalmanfilter:159' zeros(3), zeros(3), eye(3)*mag_MeasurementNoise*magUpt]; */ + /* 'attitudeKalmanfilter:161' S_k=H_k*P_apriori*H_k'+R(udpIndVect,udpIndVect); */ + /* 'attitudeKalmanfilter:162' K_k=(P_apriori*H_k'/(S_k)); */ + P_apriori_sizes[0] = 9; + P_apriori_sizes[1] = udpIndVect_sizes; + for (ib = 0; ib < 9; ib++) { + ia = udpIndVect_sizes - 1; + for (i0 = 0; i0 <= ia; i0++) { + b_A_lin[ib + 9 * i0] = 0.0F; + for (i1 = 0; i1 < 9; i1++) { + b_A_lin[ib + 9 * i0] += P_apriori[ib + 9 * i1] * (real32_T)H_k_data[i0 + + udpIndVect_sizes * i1]; + } + } + } + + ia = udpIndVect_sizes - 1; + for (ib = 0; ib <= ia; ib++) { + for (i0 = 0; i0 < 9; i0++) { + K_k_data[ib + udpIndVect_sizes * i0] = 0.0F; + for (i1 = 0; i1 < 9; i1++) { + K_k_data[ib + udpIndVect_sizes * i0] += (real32_T)H_k_data[ib + + udpIndVect_sizes * i1] * P_apriori[i1 + 9 * i0]; + } + } + } + + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[i0 + 9 * ib] = (real32_T)iv0[i0 + 3 * ib] * knownConst[5]; + } + } + + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[i0 + 9 * (ib + 3)] = 0.0F; + } + } + + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[i0 + 9 * (ib + 6)] = 0.0F; + } + } + + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[(i0 + 9 * ib) + 3] = 0.0F; + } + } + + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[(i0 + 9 * (ib + 3)) + 3] = (real32_T)iv0[i0 + 3 * ib] * knownConst[6] + * (real32_T)accUpt; + } + } + + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[(i0 + 9 * (ib + 6)) + 3] = 0.0F; + } + } + + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[(i0 + 9 * ib) + 6] = 0.0F; + } + } + + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[(i0 + 9 * (ib + 3)) + 6] = 0.0F; + } + } + + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + fv0[(i0 + 9 * (ib + 6)) + 6] = (real32_T)iv0[i0 + 3 * ib] * knownConst[7] + * (real32_T)magUpt; + } + } + + H_k_sizes[0] = udpIndVect_sizes; + H_k_sizes[1] = udpIndVect_sizes; + ia = udpIndVect_sizes - 1; + for (ib = 0; ib <= ia; ib++) { + accUpt = udpIndVect_sizes - 1; + for (i0 = 0; i0 <= accUpt; i0++) { + y = 0.0F; + for (i1 = 0; i1 < 9; i1++) { + y += K_k_data[ib + udpIndVect_sizes * i1] * (real32_T)H_k_data[i0 + + udpIndVect_sizes * i1]; + } + + A_lin[ib + H_k_sizes[0] * i0] = y + fv0[((int32_T)udpIndVect_data[ib] + + 9 * ((int32_T)udpIndVect_data[i0] - 1)) - 1]; + } + } + + mrdivide(b_A_lin, P_apriori_sizes, A_lin, H_k_sizes, K_k_data, K_k_sizes); + + /* 'attitudeKalmanfilter:165' x_aposteriori=x_apriori+K_k*y_k; */ + if ((K_k_sizes[1] == 1) || (udpIndVect_sizes == 1)) { + for (ib = 0; ib < 9; ib++) { + b_y[ib] = 0.0F; + ia = udpIndVect_sizes - 1; + for (i0 = 0; i0 <= ia; i0++) { + b_y[ib] += K_k_data[ib + K_k_sizes[0] * i0] * y_k_data[i0]; + } + } + } else { + for (accUpt = 0; accUpt < 9; accUpt++) { + b_y[accUpt] = 0.0F; + } + + magUpt = -1; + for (ib = 0; ib + 1 <= K_k_sizes[1]; ib++) { + if ((real_T)y_k_data[ib] != 0.0) { + ia = magUpt; + for (accUpt = 0; accUpt < 9; accUpt++) { + ia++; + b_y[accUpt] += y_k_data[ib] * K_k_data[ia]; + } + } + + magUpt += 9; + } + } + + for (ib = 0; ib < 9; ib++) { + x_aposteriori[ib] += b_y[ib]; + } + + /* 'attitudeKalmanfilter:166' P_aposteriori=(eye(9)-K_k*H_k)*P_apriori; */ + b_eye(dv3); + for (ib = 0; ib < 9; ib++) { + for (i0 = 0; i0 < 9; i0++) { + y = 0.0F; + ia = K_k_sizes[1] - 1; + for (i1 = 0; i1 <= ia; i1++) { + y += K_k_data[ib + K_k_sizes[0] * i1] * (real32_T)H_k_data[i1 + + udpIndVect_sizes * i0]; + } + + fv0[ib + 9 * i0] = (real32_T)dv3[ib + 9 * i0] - y; + } + } + + for (ib = 0; ib < 9; ib++) { + for (i0 = 0; i0 < 9; i0++) { + P_aposteriori[ib + 9 * i0] = 0.0F; + for (i1 = 0; i1 < 9; i1++) { + P_aposteriori[ib + 9 * i0] += fv0[ib + 9 * i1] * P_apriori[i1 + 9 * i0]; + } + } + } + } else { + /* 'attitudeKalmanfilter:167' else */ + /* 'attitudeKalmanfilter:168' x_aposteriori=x_apriori; */ + /* 'attitudeKalmanfilter:169' P_aposteriori=P_apriori; */ + memcpy((void *)&P_aposteriori[0], (void *)&P_apriori[0], 81U * sizeof + (real32_T)); } - /* %Rotation matrix generation */ - /* 'attitudeKalmanfilter:97' earth_z=x_aposteriori(1:3)/norm(x_aposteriori(1:3)); */ - B = norm(*(real32_T (*)[3])&x_aposteriori[0]); + /* %% euler anglels extraction */ + /* 'attitudeKalmanfilter:175' z_n_b = -x_aposteriori(4:6)./norm(x_aposteriori(4:6)); */ + y = norm(*(real32_T (*)[3])&x_aposteriori[3]); - /* 'attitudeKalmanfilter:98' earth_x=cross(earth_z,x_aposteriori(4:6)/norm(x_aposteriori(4:6))); */ - b_B = norm(*(real32_T (*)[3])&x_aposteriori[3]); - for (i = 0; i < 3; i++) { - earth_z[i] = x_aposteriori[i] / B; - y[i] = x_aposteriori[i + 3] / b_B; + /* 'attitudeKalmanfilter:176' m_n_b = x_aposteriori(7:9)./norm(x_aposteriori(7:9)); */ + c_y = norm(*(real32_T (*)[3])&x_aposteriori[6]); + + /* 'attitudeKalmanfilter:178' y_n_b=cross(z_n_b,m_n_b); */ + for (accUpt = 0; accUpt < 3; accUpt++) { + z_n_b[accUpt] = -x_aposteriori[accUpt + 3] / y; + x_n_b[accUpt] = x_aposteriori[accUpt + 6] / c_y; } - earth_x[0] = earth_z[1] * y[2] - earth_z[2] * y[1]; - earth_x[1] = earth_z[2] * y[0] - earth_z[0] * y[2]; - earth_x[2] = earth_z[0] * y[1] - earth_z[1] * y[0]; + cross(z_n_b, x_n_b, y_n_b); - /* 'attitudeKalmanfilter:99' earth_y=cross(earth_x,earth_z); */ - /* 'attitudeKalmanfilter:101' Rot_matrix=[earth_x,earth_y,earth_z]; */ - y[0] = earth_x[1] * earth_z[2] - earth_x[2] * earth_z[1]; - y[1] = earth_x[2] * earth_z[0] - earth_x[0] * earth_z[2]; - y[2] = earth_x[0] * earth_z[1] - earth_x[1] * earth_z[0]; - for (i = 0; i < 3; i++) { - Rot_matrix[i] = earth_x[i]; - Rot_matrix[3 + i] = y[i]; - Rot_matrix[6 + i] = earth_z[i]; + /* 'attitudeKalmanfilter:179' y_n_b=y_n_b./norm(y_n_b); */ + y = norm(y_n_b); + for (ib = 0; ib < 3; ib++) { + y_n_b[ib] /= y; } + + /* 'attitudeKalmanfilter:181' x_n_b=(cross(y_n_b,z_n_b)); */ + cross(y_n_b, z_n_b, x_n_b); + + /* 'attitudeKalmanfilter:182' x_n_b=x_n_b./norm(x_n_b); */ + y = norm(x_n_b); + for (ib = 0; ib < 3; ib++) { + /* 'attitudeKalmanfilter:188' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */ + Rot_matrix[ib] = x_n_b[ib] / y; + Rot_matrix[3 + ib] = y_n_b[ib]; + Rot_matrix[6 + ib] = z_n_b[ib]; + } + + /* 'attitudeKalmanfilter:192' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */ + /* 'attitudeKalmanfilter:193' theta=-asin(Rot_matrix(1,3)); */ + /* 'attitudeKalmanfilter:194' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */ + /* 'attitudeKalmanfilter:195' eulerAngles=[phi;theta;psi]; */ + eulerAngles[0] = rt_atan2f_snf(Rot_matrix[7], Rot_matrix[8]); + eulerAngles[1] = -(real32_T)asin(Rot_matrix[6]); + eulerAngles[2] = rt_atan2f_snf(Rot_matrix[3], Rot_matrix[0]); } /* End of code generation (attitudeKalmanfilter.c) */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h index 7aa3d048b7..8207aa5c53 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:22 2012 * */ @@ -14,6 +14,8 @@ #include #include #include +#include "rt_defines.h" +#include "rt_nonfinite.h" #include "rtwtypes.h" #include "attitudeKalmanfilter_types.h" @@ -27,6 +29,6 @@ /* Variable Definitions */ /* Function Declarations */ -extern void attitudeKalmanfilter(real32_T dt, const real32_T z_k[9], const real32_T x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T knownConst[7], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T P_aposteriori[144]); +extern void attitudeKalmanfilter(real32_T dt, const int8_T updVect[9], const real32_T z_k_data[9], const int32_T z_k_sizes[1], const real32_T u[4], const real32_T x_aposteriori_k[9], const real32_T P_aposteriori_k[81], const real32_T knownConst[20], real32_T eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[9], real32_T P_aposteriori[81]); #endif /* End of code generation (attitudeKalmanfilter.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c index 2800191c32..abf1519a67 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter_initialize' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:23 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h index d2d97bb3b9..20186f183c 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter_initialize' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:24 2012 * */ @@ -14,6 +14,8 @@ #include #include #include +#include "rt_defines.h" +#include "rt_nonfinite.h" #include "rtwtypes.h" #include "attitudeKalmanfilter_types.h" diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_ref.rsp b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_ref.rsp deleted file mode 100755 index e69de29bb2..0000000000 diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.bat b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.bat deleted file mode 100755 index 76fb78ca75..0000000000 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.bat +++ /dev/null @@ -1,11 +0,0 @@ -@echo off -call "%VS100COMNTOOLS%..\..\VC\vcvarsall.bat" AMD64 - -cd . -nmake -f attitudeKalmanfilter_rtw.mk GENERATE_REPORT=1 ADD_MDL_NAME_TO_GLOBALS=0 -@if errorlevel 1 goto error_exit -exit /B 0 - -:error_exit -echo The make command returned an error of %errorlevel% -An_error_occurred_during_the_call_to_make diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.mk b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.mk deleted file mode 100755 index b2123704b1..0000000000 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.mk +++ /dev/null @@ -1,328 +0,0 @@ -# Copyright 1994-2010 The MathWorks, Inc. -# -# File : xrt_vcx64.tmf $Revision: 1.1.6.1 $ -# -# Abstract: -# Code generation template makefile for building a Windows-based, -# stand-alone real-time version of MATLAB-code using generated C/C++ -# code and the -# Microsoft Visual C/C++ compiler version 8, 9, 10 for x64 -# -# Note that this template is automatically customized by the -# code generation build procedure to create ".mk" -# -# The following defines can be used to modify the behavior of the -# build: -# -# OPT_OPTS - Optimization option. See DEFAULT_OPT_OPTS in -# vctools.mak for default. -# OPTS - User specific options. -# CPP_OPTS - C++ compiler options. -# USER_SRCS - Additional user sources, such as files needed by -# S-functions. -# USER_INCLUDES - Additional include paths -# (i.e. USER_INCLUDES="-Iwhere-ever -Iwhere-ever2") -# -# To enable debugging: -# set OPT_OPTS=-Zi (may vary with compiler version, see compiler doc) -# modify tmf LDFLAGS to include /DEBUG -# - -#------------------------ Macros read by make_rtw ----------------------------- -# -# The following macros are read by the code generation build procedure: -# -# MAKECMD - This is the command used to invoke the make utility -# HOST - What platform this template makefile is targeted for -# (i.e. PC or UNIX) -# BUILD - Invoke make from the code generation build procedure -# (yes/no)? -# SYS_TARGET_FILE - Name of system target file. - -MAKECMD = nmake -HOST = PC -BUILD = yes -SYS_TARGET_FILE = ert.tlc -BUILD_SUCCESS = ^#^#^# Created -COMPILER_TOOL_CHAIN = vcx64 - -#---------------------- Tokens expanded by make_rtw --------------------------- -# -# The following tokens, when wrapped with "|>" and "<|" are expanded by the -# code generation build procedure. -# -# MODEL_NAME - Name of the Simulink block diagram -# MODEL_MODULES - Any additional generated source modules -# MAKEFILE_NAME - Name of makefile created from template makefile .mk -# MATLAB_ROOT - Path to where MATLAB is installed. -# MATLAB_BIN - Path to MATLAB executable. -# S_FUNCTIONS - List of S-functions. -# S_FUNCTIONS_LIB - List of S-functions libraries to link. -# SOLVER - Solver source file name -# NUMST - Number of sample times -# TID01EQ - yes (1) or no (0): Are sampling rates of continuous task -# (tid=0) and 1st discrete task equal. -# NCSTATES - Number of continuous states -# BUILDARGS - Options passed in at the command line. -# MULTITASKING - yes (1) or no (0): Is solver mode multitasking -# EXT_MODE - yes (1) or no (0): Build for external mode -# TMW_EXTMODE_TESTING - yes (1) or no (0): Build ext_test.c for external mode -# testing. -# EXTMODE_TRANSPORT - Index of transport mechanism (e.g. tcpip, serial) for extmode -# EXTMODE_STATIC - yes (1) or no (0): Use static instead of dynamic mem alloc. -# EXTMODE_STATIC_SIZE - Size of static memory allocation buffer. - -MODEL = attitudeKalmanfilter -MODULES = attitudeKalmanfilter.c eye.c mrdivide.c norm.c attitudeKalmanfilter_initialize.c attitudeKalmanfilter_terminate.c rt_nonfinite.c rtGetNaN.c rtGetInf.c -MAKEFILE = attitudeKalmanfilter_rtw.mk -MATLAB_ROOT = C:\Program Files\MATLAB\R2011a -ALT_MATLAB_ROOT = C:\PROGRA~1\MATLAB\R2011a -MATLAB_BIN = C:\Program Files\MATLAB\R2011a\bin -ALT_MATLAB_BIN = C:\PROGRA~1\MATLAB\R2011a\bin -S_FUNCTIONS = -S_FUNCTIONS_LIB = -SOLVER = -NUMST = 1 -TID01EQ = 0 -NCSTATES = 0 -BUILDARGS = GENERATE_REPORT=1 ADD_MDL_NAME_TO_GLOBALS=0 -MULTITASKING = 0 -EXT_MODE = 0 -TMW_EXTMODE_TESTING = 0 -EXTMODE_TRANSPORT = 0 -EXTMODE_STATIC = 0 -EXTMODE_STATIC_SIZE = 1000000 - -MODELREFS = -SHARED_SRC = -SHARED_SRC_DIR = -SHARED_BIN_DIR = -SHARED_LIB = -TARGET_LANG_EXT = c -OPTIMIZATION_FLAGS = /O2 /Oy- -ADDITIONAL_LDFLAGS = - -#--------------------------- Model and reference models ----------------------- -MODELLIB = attitudeKalmanfilter.lib -MODELREF_LINK_LIBS = -MODELREF_LINK_RSPFILE = attitudeKalmanfilter_ref.rsp -MODELREF_INC_PATH = -RELATIVE_PATH_TO_ANCHOR = F:\CODEGE~1 -MODELREF_TARGET_TYPE = RTW - -!if "$(MATLAB_ROOT)" != "$(ALT_MATLAB_ROOT)" -MATLAB_ROOT = $(ALT_MATLAB_ROOT) -!endif -!if "$(MATLAB_BIN)" != "$(ALT_MATLAB_BIN)" -MATLAB_BIN = $(ALT_MATLAB_BIN) -!endif - -#--------------------------- Tool Specifications ------------------------------ - -CPU = AMD64 -!include $(MATLAB_ROOT)\rtw\c\tools\vctools.mak -APPVER = 5.02 - -PERL = $(MATLAB_ROOT)\sys\perl\win32\bin\perl -#------------------------------ Include/Lib Path ------------------------------ - -MATLAB_INCLUDES = $(MATLAB_ROOT)\simulink\include -MATLAB_INCLUDES = $(MATLAB_INCLUDES);$(MATLAB_ROOT)\extern\include -MATLAB_INCLUDES = $(MATLAB_INCLUDES);$(MATLAB_ROOT)\rtw\c\src -MATLAB_INCLUDES = $(MATLAB_INCLUDES);$(MATLAB_ROOT)\rtw\c\src\ext_mode\common - -# Additional file include paths - -MATLAB_INCLUDES = $(MATLAB_INCLUDES);F:\codegenerationMatlabAttFilter\codegen\lib\attitudeKalmanfilter -MATLAB_INCLUDES = $(MATLAB_INCLUDES);F:\codegenerationMatlabAttFilter - -INCLUDE = .;$(RELATIVE_PATH_TO_ANCHOR);$(MATLAB_INCLUDES);$(INCLUDE);$(MODELREF_INC_PATH) - -!if "$(SHARED_SRC_DIR)" != "" -INCLUDE = $(INCLUDE);$(SHARED_SRC_DIR) -!endif - -#------------------------ External mode --------------------------------------- -# Uncomment -DVERBOSE to have information printed to stdout -# To add a new transport layer, see the comments in -# /toolbox/simulink/simulink/extmode_transports.m -!if $(EXT_MODE) == 1 -EXT_CC_OPTS = -DEXT_MODE # -DVERBOSE -!if $(EXTMODE_TRANSPORT) == 0 #tcpip -EXT_SRC = updown.c rtiostream_interface.c rtiostream_tcpip.c -EXT_LIB = wsock32.lib -!endif -!if $(EXTMODE_TRANSPORT) == 1 #serial_win32 -EXT_SRC = ext_svr.c updown.c ext_work.c ext_svr_serial_transport.c -EXT_SRC = $(EXT_SRC) ext_serial_pkt.c ext_serial_win32_port.c -EXT_LIB = -!endif -!if $(TMW_EXTMODE_TESTING) == 1 -EXT_SRC = $(EXT_SRC) ext_test.c -EXT_CC_OPTS = $(EXT_CC_OPTS) -DTMW_EXTMODE_TESTING -!endif -!if $(EXTMODE_STATIC) == 1 -EXT_SRC = $(EXT_SRC) mem_mgr.c -EXT_CC_OPTS = $(EXT_CC_OPTS) -DEXTMODE_STATIC -DEXTMODE_STATIC_SIZE=$(EXTMODE_STATIC_SIZE) -!endif -!else -EXT_SRC = -EXT_CC_OPTS = -EXT_LIB = -!endif - -#------------------------ rtModel ---------------------------------------------- - -RTM_CC_OPTS = -DUSE_RTMODEL - -#----------------- Compiler and Linker Options -------------------------------- - -# Optimization Options -# Set OPT_OPTS=-Zi for debugging (may depend on compiler version) -OPT_OPTS = $(DEFAULT_OPT_OPTS) - -# General User Options -OPTS = - -!if "$(OPTIMIZATION_FLAGS)" != "" -CC_OPTS = $(OPTS) $(EXT_CC_OPTS) $(RTM_CC_OPTS) $(OPTIMIZATION_FLAGS) -!else -CC_OPTS = $(OPT_OPTS) $(OPTS) $(EXT_CC_OPTS) $(RTM_CC_OPTS) -!endif - -CPP_REQ_DEFINES = -DMODEL=$(MODEL) -DRT -DNUMST=$(NUMST) \ - -DTID01EQ=$(TID01EQ) -DNCSTATES=$(NCSTATES) \ - -DMT=$(MULTITASKING) -DHAVESTDIO - -# Uncomment this line to move warning level to W4 -# cflags = $(cflags:W3=W4) -CFLAGS = $(CC_OPTS) $(CPP_REQ_DEFINES) $(USER_INCLUDES) \ - $(cflags) $(cvarsmt) /wd4996 -CPPFLAGS = $(CPP_OPTS) $(CC_OPTS) $(CPP_REQ_DEFINES) $(USER_INCLUDES) \ - $(cflags) $(cvarsmt) /wd4996 /EHsc- -LDFLAGS = $(ldebug) $(conflags) $(EXT_LIB) $(conlibs) libcpmt.lib $(ADDITIONAL_LDFLAGS) - -# libcpmt.lib is the multi-threaded, static lib version of the C++ standard lib - -#----------------------------- Source Files ----------------------------------- - - -#Standalone executable -!if "$(MODELREF_TARGET_TYPE)" == "NONE" -PRODUCT = $(RELATIVE_PATH_TO_ANCHOR)\$(MODEL).exe -REQ_SRCS = $(MODULES) $(EXT_SRC) - -#Model Reference Target -!else -PRODUCT = $(MODELLIB) -REQ_SRCS = $(MODULES) $(EXT_SRC) -!endif - -USER_SRCS = - -SRCS = $(REQ_SRCS) $(USER_SRCS) $(S_FUNCTIONS) -OBJS_CPP_UPPER = $(SRCS:.CPP=.obj) -OBJS_CPP_LOWER = $(OBJS_CPP_UPPER:.cpp=.obj) -OBJS_C_UPPER = $(OBJS_CPP_LOWER:.C=.obj) -OBJS = $(OBJS_C_UPPER:.c=.obj) -SHARED_OBJS = $(SHARED_SRC:.c=.obj) -# ------------------------- Additional Libraries ------------------------------ - -LIBS = - - -LIBS = $(LIBS) - -# ---------------------------- Linker Script ---------------------------------- - -CMD_FILE = $(MODEL).lnk -GEN_LNK_SCRIPT = $(MATLAB_ROOT)\rtw\c\tools\mkvc_lnk.pl - -#--------------------------------- Rules -------------------------------------- -all: set_environment_variables $(PRODUCT) - -!if "$(MODELREF_TARGET_TYPE)" == "NONE" -#--- Stand-alone model --- -$(PRODUCT) : $(OBJS) $(SHARED_LIB) $(LIBS) $(MODELREF_LINK_LIBS) - @cmd /C "echo ### Linking ..." - $(PERL) $(GEN_LNK_SCRIPT) $(CMD_FILE) $(OBJS) - $(LD) $(LDFLAGS) $(S_FUNCTIONS_LIB) $(SHARED_LIB) $(LIBS) $(MAT_LIBS) @$(CMD_FILE) @$(MODELREF_LINK_RSPFILE) -out:$@ - @del $(CMD_FILE) - @cmd /C "echo $(BUILD_SUCCESS) executable $(MODEL).exe" -!else -#--- Model reference code generation Target --- -$(PRODUCT) : $(OBJS) $(SHARED_LIB) - @cmd /C "echo ### Linking ..." - $(PERL) $(GEN_LNK_SCRIPT) $(CMD_FILE) $(OBJS) - $(LD) -lib /OUT:$(PRODUCT) @$(CMD_FILE) $(S_FUNCTIONS_LIB) - @cmd /C "echo $(BUILD_SUCCESS) static library $(MODELLIB)" -!endif - -{$(MATLAB_ROOT)\rtw\c\src}.c.obj : - @cmd /C "echo ### Compiling $<" - $(CC) $(CFLAGS) $< - -{$(MATLAB_ROOT)\rtw\c\src\ext_mode\common}.c.obj : - @cmd /C "echo ### Compiling $<" - $(CC) $(CFLAGS) $< - -{$(MATLAB_ROOT)\rtw\c\src\rtiostream\rtiostreamtcpip}.c.obj : - @cmd /C "echo ### Compiling $<" - $(CC) $(CFLAGS) $< - -{$(MATLAB_ROOT)\rtw\c\src\ext_mode\serial}.c.obj : - @cmd /C "echo ### Compiling $<" - $(CC) $(CFLAGS) $< - -{$(MATLAB_ROOT)\rtw\c\src\ext_mode\custom}.c.obj : - @cmd /C "echo ### Compiling $<" - $(CC) $(CFLAGS) $< - -# Additional sources - - - - - -# Put these rule last, otherwise nmake will check toolboxes first - -{$(RELATIVE_PATH_TO_ANCHOR)}.c.obj : - @cmd /C "echo ### Compiling $<" - $(CC) $(CFLAGS) $< - -{$(RELATIVE_PATH_TO_ANCHOR)}.cpp.obj : - @cmd /C "echo ### Compiling $<" - $(CC) $(CPPFLAGS) $< - -.c.obj : - @cmd /C "echo ### Compiling $<" - $(CC) $(CFLAGS) $< - -.cpp.obj : - @cmd /C "echo ### Compiling $<" - $(CC) $(CPPFLAGS) $< - -!if "$(SHARED_LIB)" != "" -$(SHARED_LIB) : $(SHARED_SRC) - @cmd /C "echo ### Creating $@" - @$(CC) $(CFLAGS) -Fo$(SHARED_BIN_DIR)\ @<< -$? -<< - @$(LIBCMD) /nologo /out:$@ $(SHARED_OBJS) - @cmd /C "echo ### $@ Created" -!endif - - -set_environment_variables: - @set INCLUDE=$(INCLUDE) - @set LIB=$(LIB) - -# Libraries: - - - - - -#----------------------------- Dependencies ----------------------------------- - -$(OBJS) : $(MAKEFILE) rtw_proj.tmw diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c index eab8c7d6e0..458ddb9eb0 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter_terminate' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:24 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h index fafd866e42..a939eee200 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter_terminate' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:24 2012 * */ @@ -14,6 +14,8 @@ #include #include #include +#include "rt_defines.h" +#include "rt_nonfinite.h" #include "rtwtypes.h" #include "attitudeKalmanfilter_types.h" diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h index 05f4664cd9..599a168848 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:22 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/buildInfo.mat b/apps/attitude_estimator_ekf/codegen/buildInfo.mat deleted file mode 100755 index b811d003977e5a3d432f33c24d297eddea2b494b..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 4114 zcma)8X*3iJ*d4n`w(JZeZ%DSHv4tT~Dnvu}HA}X!4_SsmmTD~7vWFK+=f`uNd(V09J?C1P!fu*gmWODZm$x#7sr%gv_EnR= z73>w|2agCbkiTVi$I@EoqME!}gs)eW?_GI#sDb<)-@EeHql4uyT#!GnXP~WT0D;JB zoxh+Z|Nq3WAmD$&W@+_rT;~G-V#{Zo?je>&#bj0N*6NDZ7tp&4MQX~cx5w+&_1>sy z%JbT_UCU)Rv9w;S5dyjdpUr=c)eBsSvciyEFqL@ZG<1f1ELCy({J<>8uog^zuE#Uj zbBVed7ds>+w03%Q0Xi=gpXVV+Ie>`EiZBwMR=Lz#cZK|wqq0dXhxE_yaoZXW*(|L% zo&`{apt=JHg*hACFR1s;HB`S|8O6ha#^XmlP-crCoyfVnj%hbqkBKQOCX6{bEs0FY)~4p4if+?F=_$&;mIl&o61m=dW{?c_%?G zI@OIW=4#05i=%Hk0>T1^yiFp}Q-ZOYs)9$grdL#A9t5wD(d_j(F+)uBDx7DX6MH$$ z=hHp{^&0w$f6%(02L$Ad9~jhGl6rdy9P_ni@uwhy<|}E;Pvsuid-vlHy%$&Eu6%OT zOP^PrkZ-j&S~l4>QqBxmSj*Z$x7uzG_7~E+6j*afcn9yV*KfKsiHGu-jjD=A;kvZ8 z(<|C!I-af)q$o$K(TTSk%h zcJJ6C2gs=#Ijj)an(ZIw&ebs7;@ls8JSO_~^rMM@%nD*{gU_&fx4UImPqq`W?Z=E^ zMeWae{Pa2ZsP@C<%NpmhbiQ}0$rO1VlpO@uy0^PI1e6rk;NYn6pgh!R>9y_mZ~gRr z?2L7dOI%hc#$*VnxH#=Y1O?P&pO-mQE;gZ)U87!xZZ^ZKmcBj7R}T@WJ(Zc=_Qme@ zmhB=<4?HlKsi(AN1Xx#5WTH-i?bhZCY5a;w8Perr%IcM{Fsc4(qE@9HMTNb}p=CeT z_Wj3oMHDibxXyftQ|0uSbKU@-p=on^Oy+$=781cqw0-^h{Tv}iEhkHIptwr55Lr&F zlBX#H`xt7CjN6>7s04fH9sjjg^uxRyi&-R^0=+@m0y6bcETsYDLt>ROO@fzcjzVS; zJ(Os7c$xMnmd*f6VW8iFBEif2Lu;IbqlsJVuMEh#ysmrEnkqCac4RRYE=-H(?7K$n z0w!c($wD-(6oMipo-?Tm3l@53LF8he2liYea;FsY_S__@a)zFwnX!8)Qck4Y0WK?0 zq*LaB36HTM>$7N|WX4_mN6!H6z@kT;fl{4Un%qAXt(vaU}Vh9C6bsK2k36|9O&BW5kyfNkr%j z!Q~i}DTFd2Rh(8aMd=14wukkX5X>pv1>X=-^i3>WfYFjnm_6Cw^^B}WtKfpVQs=c8 z;?;0D#^wOofVN@B2&?GhNc0x?-X~xAYrnl)idws;L>f+bzRS0N#IebeyRnJPQ#?S* z?0cXXPGW8v;z%NG5Pm;sHs{4?NiD8YqG}1wYVQ^3C7m`1-AW zEn0JO)zfcOA%??1D|?bXrCo~=IE?`AQBTok2jJ>MQxE#ai3lkMh!UyIke)_x{nKke z3#p@7p$JFQ$^;oDu|8M)gyfKaqvKS>Zf3M9!7Y-ds0lu}YwW@H*s+#;S!TYKr^f}` z&9%CJWVqdT0%Ja{j@paKp5$3MMJm#Ya;fg-z+6%>XforA8n1S7U@T#~`FgsEngoek z63KhUq`mgLtoZVpcyN&TI3PY60`BOYN(<)Rd)JmwYI4;VLq(tNRBk@u0chgi-j#PJJED&jO1avcBunt58h;H0D-yt18{Ygc)>&8T65+ zNABnBwIXfAYiuWR8DvcBt6)4*Cl#YYv-fnXgM=^^*?V2pF3M5+MlwI?m!?C63{u8B zS@Gs9o}y1b!MA_2vmPlPym?7tajruZMvgnn98$ec|m(i-B~n z1>n_P&sj2)2LNEPc8XnW2G2`A5EeEsj~y1tjy0~4aF>F=JFPT2d_%LSM62xoYe8dpn;yF4GrwVvD6Kre;?f!Qz@$n(4i5%)4?^e;NM)@~jG%9arx*WgTK{ z2i^L&#)qEIqk!1R+3R8ps}4#%#=Anh!m>iL!WBXl!n0!Y9=4xu8+o`ZQTo{ay>hpB zHxz$f^g&nFX=bw0z+3AwotElT1P>o#eQ4RN)%7*&)=(w!HGwNnt?75@g5v z9q^1s`h4?}*ej#t*9>$#7S4N%BxEHrPm#}0R$5|o&+Z$STTq3?zFTUkxS8mHtB*a1A8b8U5bT!WT z8yJ(Pl~Kg0-P{E zNOZkqt4#x4^wr+6knO26l*{)!scWos|Z&dhktO_{foj=~?n6CaT$GRYX}Zoa*gBO+TYuUra_2wdQ@8IX(RuH;}pfd!bG znEqhj*!7saQ|Bn!sqixq%p3YptDP_H{nH~qBP8B{fG>76Hc1}`>hhaPwm-I{DTVJX zPOmLp%d~l?w{3%s&-**Itn1Rp+Mr_0q>SZxU^BqhF&>@QkFs~~?v)MSk91CWpLYNI z(!jT-o#Jn;5bJWS4OD+4zA1q)oko07iMP0J_Gz{yH}A!>fr#t^ z(nJ7*tt0w#^uMZ)0Q{@^>pq^IC4k{zlTU2JYA1!TCpNU%UrQ8-1J?3qmQm%y`7pxp z&P>Orav0l-o_712CbmiUZM|DiGPmC8jm}>f#(h>%8XLn-pTAIl1`1VH=~qzFhbngR zUKcMcz@5RtL<^I{5}YE^pJusRhDr}70qT|hyZn#-3;RF%S%(R0nF~TOm#;s}y@hVh zZ3NxtGPSX{G2vAVWM292tJ#_FJJzRk-Qzc+1Mm{}z_mG_airR)n>{i(VB0kE#_FHg z%`)f|8u3tQ-}gywbOLnH=}h}C>=!QoP~wh_q~OwPYpwUE;=B*Z*+qmJVIlaHsFNI{ zURj8_#CU;5O%bpbCtG&li;qHIQ8wBZ$ui1z=CC6jV(ogVcFDDi8;lYa(LD6Es{?=n zRNIoFlL^dMw6-ho!wOa_aBaUlV*cGpWsrj;kLeM};Lm8@hl!U3KB}TNQDy=^YwBUz zQU;kmMZ*@M`sio5ComBwYYb4H8p)$6RRhG2X%DYih}H&;B?l^{55;OnHq_SXi%DFt z!oc`VtJL@CO(zUNTM#0r<+A@TIL z@aZm@$J$UTs8?phx0mZG{Bz4*s;cAiLzlH!(uf!7EBdf$CVwSbunPU&?adbgi)(8O zhbxp_?ve?!V76yy|8Ar&@Si)+m0)r1Zn4_M8w~V+3C(KCN+o^;?KWW*oRgWroyTXZ zf2Ze;nM^-Q;KG=@{CK0m{f?m;?c`pzZpVz)`;XPDSFE{}%X0pNfKU9ramRo^(d1_B z=kmXW1%CxB@tV_6cTo{kI?zv_49GRk|&F|>}cuK2Qp}QbkgJq2D z=C9TcqDNL*EPv?Y0S4IhV~(7dSGb*FJhSuqzOXpHDJRh$FXx7Y9M}|D0{{f59RFymjO^0p8Z!yq@X%QYm`u z*HZx^1yfkk555{&@&bn?R7Cvv(KWi0l%wRrvjm}vyprPlCnqlOZ~_$Wa0&bm*2WhF diff --git a/apps/attitude_estimator_ekf/codegen/cross.c b/apps/attitude_estimator_ekf/codegen/cross.c new file mode 100755 index 0000000000..49f655ece8 --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/cross.c @@ -0,0 +1,37 @@ +/* + * cross.c + * + * Code generation for function 'cross' + * + * C source code generated on: Mon Sep 17 20:13:23 2012 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "attitudeKalmanfilter.h" +#include "cross.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ + +/* + * + */ +void cross(const real32_T a[3], const real32_T b[3], real32_T c[3]) +{ + c[0] = a[1] * b[2] - a[2] * b[1]; + c[1] = a[2] * b[0] - a[0] * b[2]; + c[2] = a[0] * b[1] - a[1] * b[0]; +} + +/* End of code generation (cross.c) */ diff --git a/apps/attitude_estimator_ekf/codegen/cross.h b/apps/attitude_estimator_ekf/codegen/cross.h new file mode 100755 index 0000000000..844d8138fd --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/cross.h @@ -0,0 +1,34 @@ +/* + * cross.h + * + * Code generation for function 'cross' + * + * C source code generated on: Mon Sep 17 20:13:23 2012 + * + */ + +#ifndef __CROSS_H__ +#define __CROSS_H__ +/* Include files */ +#include +#include +#include +#include +#include "rt_defines.h" +#include "rt_nonfinite.h" + +#include "rtwtypes.h" +#include "attitudeKalmanfilter_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void cross(const real32_T a[3], const real32_T b[3], real32_T c[3]); +#endif +/* End of code generation (cross.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/diag.c b/apps/attitude_estimator_ekf/codegen/diag.c new file mode 100755 index 0000000000..e54d52a384 --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/diag.c @@ -0,0 +1,42 @@ +/* + * diag.c + * + * Code generation for function 'diag' + * + * C source code generated on: Mon Sep 17 20:13:22 2012 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "attitudeKalmanfilter.h" +#include "diag.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ + +/* + * + */ +void diag(const real32_T v[3], real32_T d[9]) +{ + int32_T j; + for (j = 0; j < 9; j++) { + d[j] = 0.0F; + } + + for (j = 0; j < 3; j++) { + d[j + 3 * j] = v[j]; + } +} + +/* End of code generation (diag.c) */ diff --git a/apps/attitude_estimator_ekf/codegen/diag.h b/apps/attitude_estimator_ekf/codegen/diag.h new file mode 100755 index 0000000000..a4a2a4c829 --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/diag.h @@ -0,0 +1,34 @@ +/* + * diag.h + * + * Code generation for function 'diag' + * + * C source code generated on: Mon Sep 17 20:13:23 2012 + * + */ + +#ifndef __DIAG_H__ +#define __DIAG_H__ +/* Include files */ +#include +#include +#include +#include +#include "rt_defines.h" +#include "rt_nonfinite.h" + +#include "rtwtypes.h" +#include "attitudeKalmanfilter_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void diag(const real32_T v[3], real32_T d[9]); +#endif +/* End of code generation (diag.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/eye.c b/apps/attitude_estimator_ekf/codegen/eye.c index e4655839c4..aece401c25 100755 --- a/apps/attitude_estimator_ekf/codegen/eye.c +++ b/apps/attitude_estimator_ekf/codegen/eye.c @@ -3,7 +3,7 @@ * * Code generation for function 'eye' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:23 2012 * */ @@ -27,12 +27,12 @@ /* * */ -void b_eye(real_T I[144]) +void b_eye(real_T I[81]) { int32_T i; - memset((void *)&I[0], 0, 144U * sizeof(real_T)); - for (i = 0; i < 12; i++) { - I[i + 12 * i] = 1.0; + memset((void *)&I[0], 0, 81U * sizeof(real_T)); + for (i = 0; i < 9; i++) { + I[i + 9 * i] = 1.0; } } diff --git a/apps/attitude_estimator_ekf/codegen/eye.h b/apps/attitude_estimator_ekf/codegen/eye.h index e8881747f6..e715ae1c39 100755 --- a/apps/attitude_estimator_ekf/codegen/eye.h +++ b/apps/attitude_estimator_ekf/codegen/eye.h @@ -3,7 +3,7 @@ * * Code generation for function 'eye' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:23 2012 * */ @@ -14,6 +14,8 @@ #include #include #include +#include "rt_defines.h" +#include "rt_nonfinite.h" #include "rtwtypes.h" #include "attitudeKalmanfilter_types.h" @@ -27,7 +29,7 @@ /* Variable Definitions */ /* Function Declarations */ -extern void b_eye(real_T I[144]); +extern void b_eye(real_T I[81]); extern void eye(real_T I[9]); #endif /* End of code generation (eye.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/find.c b/apps/attitude_estimator_ekf/codegen/find.c new file mode 100755 index 0000000000..532ba43970 --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/find.c @@ -0,0 +1,77 @@ +/* + * find.c + * + * Code generation for function 'find' + * + * C source code generated on: Mon Sep 17 20:13:22 2012 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "attitudeKalmanfilter.h" +#include "find.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ + +/* + * + */ +void find(const int8_T x[9], real_T i_data[9], int32_T i_sizes[1]) +{ + int32_T idx; + int32_T ii; + boolean_T exitg1; + boolean_T guard1 = FALSE; + int32_T i2; + int8_T b_i_data[9]; + idx = 0; + i_sizes[0] = 9; + ii = 1; + exitg1 = 0U; + while ((exitg1 == 0U) && (ii <= 9)) { + guard1 = FALSE; + if (x[ii - 1] != 0) { + idx++; + i_data[idx - 1] = (real_T)ii; + if (idx >= 9) { + exitg1 = 1U; + } else { + guard1 = TRUE; + } + } else { + guard1 = TRUE; + } + + if (guard1 == TRUE) { + ii++; + } + } + + if (1 > idx) { + idx = 0; + } + + ii = idx - 1; + for (i2 = 0; i2 <= ii; i2++) { + b_i_data[i2] = (int8_T)i_data[i2]; + } + + i_sizes[0] = idx; + ii = idx - 1; + for (i2 = 0; i2 <= ii; i2++) { + i_data[i2] = (real_T)b_i_data[i2]; + } +} + +/* End of code generation (find.c) */ diff --git a/apps/attitude_estimator_ekf/codegen/find.h b/apps/attitude_estimator_ekf/codegen/find.h new file mode 100755 index 0000000000..ceb90b6cf6 --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/find.h @@ -0,0 +1,34 @@ +/* + * find.h + * + * Code generation for function 'find' + * + * C source code generated on: Mon Sep 17 20:13:22 2012 + * + */ + +#ifndef __FIND_H__ +#define __FIND_H__ +/* Include files */ +#include +#include +#include +#include +#include "rt_defines.h" +#include "rt_nonfinite.h" + +#include "rtwtypes.h" +#include "attitudeKalmanfilter_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void find(const int8_T x[9], real_T i_data[9], int32_T i_sizes[1]); +#endif +/* End of code generation (find.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/mrdivide.c b/apps/attitude_estimator_ekf/codegen/mrdivide.c index cb81b53282..2fcaf8cb67 100755 --- a/apps/attitude_estimator_ekf/codegen/mrdivide.c +++ b/apps/attitude_estimator_ekf/codegen/mrdivide.c @@ -3,7 +3,7 @@ * * Code generation for function 'mrdivide' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:23 2012 * */ @@ -21,136 +21,719 @@ /* Variable Definitions */ /* Function Declarations */ +static real32_T b_eml_matlab_zlarfg(real32_T *alpha1, const real32_T *x); +static void eml_lusolve(const real32_T A_data[81], const int32_T A_sizes[2], + real32_T B_data[81], int32_T B_sizes[2]); +static real32_T eml_matlab_zlarfg(int32_T n, real32_T *alpha1, real32_T x_data + [81], int32_T x_sizes[2], int32_T ix0); +static void eml_qrsolve(const real32_T A_data[81], const int32_T A_sizes[2], + real32_T B_data[81], int32_T B_sizes[2], real32_T Y_data[81], int32_T Y_sizes + [2]); +static real32_T eml_xnrm2(int32_T n, const real32_T x_data[81], const int32_T + x_sizes[2], int32_T ix0); /* Function Definitions */ /* * */ -void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108]) +static real32_T b_eml_matlab_zlarfg(real32_T *alpha1, const real32_T *x) { - int32_T jy; + return 0.0F; +} + +/* + * + */ +static void eml_lusolve(const real32_T A_data[81], const int32_T A_sizes[2], + real32_T B_data[81], int32_T B_sizes[2]) +{ + int32_T loop_ub; int32_T iy; - real32_T b_A[81]; - int8_T ipiv[9]; + real32_T b_A_data[81]; + int32_T jA; + int32_T tmp_data[9]; + int32_T ipiv_data[9]; + int32_T ldap1; int32_T j; int32_T mmj; int32_T jj; - int32_T jp1j; - int32_T c; int32_T ix; real32_T temp; int32_T k; real32_T s; - int32_T loop_ub; - real32_T Y[108]; - for (jy = 0; jy < 9; jy++) { - for (iy = 0; iy < 9; iy++) { - b_A[iy + 9 * jy] = B[jy + 9 * iy]; - } - - ipiv[jy] = (int8_T)(1 + jy); + int32_T jrow; + int32_T b_loop_ub; + int32_T c; + loop_ub = A_sizes[0] * A_sizes[1] - 1; + for (iy = 0; iy <= loop_ub; iy++) { + b_A_data[iy] = A_data[iy]; } - for (j = 0; j < 8; j++) { - mmj = -j; - jj = j * 10; - jp1j = jj + 1; - c = mmj + 9; - jy = 0; - ix = jj; - temp = fabsf(b_A[jj]); - for (k = 2; k <= c; k++) { - ix++; - s = fabsf(b_A[ix]); - if (s > temp) { - jy = k - 1; - temp = s; + iy = A_sizes[1]; + jA = A_sizes[1]; + jA = iy <= jA ? iy : jA; + if (jA < 1) { + } else { + loop_ub = jA - 1; + for (iy = 0; iy <= loop_ub; iy++) { + tmp_data[iy] = 1 + iy; + } + + loop_ub = jA - 1; + for (iy = 0; iy <= loop_ub; iy++) { + ipiv_data[iy] = tmp_data[iy]; + } + } + + ldap1 = A_sizes[1] + 1; + iy = A_sizes[1] - 1; + jA = A_sizes[1]; + loop_ub = iy <= jA ? iy : jA; + for (j = 1; j <= loop_ub; j++) { + mmj = (A_sizes[1] - j) + 1; + jj = (j - 1) * ldap1; + if (mmj < 1) { + jA = -1; + } else { + jA = 0; + if (mmj > 1) { + ix = jj; + temp = (real32_T)fabs(b_A_data[jj]); + for (k = 1; k + 1 <= mmj; k++) { + ix++; + s = (real32_T)fabs(b_A_data[ix]); + if (s > temp) { + jA = k; + temp = s; + } + } } } - if ((real_T)b_A[jj + jy] != 0.0) { - if (jy != 0) { - ipiv[j] = (int8_T)((j + jy) + 1); - ix = j; - iy = j + jy; - for (k = 0; k < 9; k++) { - temp = b_A[ix]; - b_A[ix] = b_A[iy]; - b_A[iy] = temp; - ix += 9; - iy += 9; + if ((real_T)b_A_data[jj + jA] != 0.0) { + if (jA != 0) { + ipiv_data[j - 1] = j + jA; + jrow = j - 1; + iy = jrow + jA; + for (k = 1; k <= A_sizes[1]; k++) { + temp = b_A_data[jrow]; + b_A_data[jrow] = b_A_data[iy]; + b_A_data[iy] = temp; + jrow += A_sizes[1]; + iy += A_sizes[1]; } } - loop_ub = (jp1j + mmj) + 8; - for (iy = jp1j; iy + 1 <= loop_ub; iy++) { - b_A[iy] /= b_A[jj]; + b_loop_ub = jj + mmj; + for (jA = jj + 1; jA + 1 <= b_loop_ub; jA++) { + b_A_data[jA] /= b_A_data[jj]; } } - c = 8 - j; - jy = jj + 9; - for (iy = 1; iy <= c; iy++) { - if ((real_T)b_A[jy] != 0.0) { - temp = b_A[jy] * -1.0F; - ix = jp1j; - loop_ub = (mmj + jj) + 18; - for (k = 10 + jj; k + 1 <= loop_ub; k++) { - b_A[k] += b_A[ix] * temp; + c = A_sizes[1] - j; + jA = jj + ldap1; + iy = jj + A_sizes[1]; + for (jrow = 1; jrow <= c; jrow++) { + if ((real_T)b_A_data[iy] != 0.0) { + temp = b_A_data[iy] * -1.0F; + ix = jj; + b_loop_ub = (mmj + jA) - 1; + for (k = jA; k + 1 <= b_loop_ub; k++) { + b_A_data[k] += b_A_data[ix + 1] * temp; ix++; } } - jy += 9; - jj += 9; + iy += A_sizes[1]; + jA += A_sizes[1]; } } - for (jy = 0; jy < 12; jy++) { - for (iy = 0; iy < 9; iy++) { - Y[iy + 9 * jy] = A[jy + 12 * iy]; - } - } - - for (iy = 0; iy < 9; iy++) { - if (ipiv[iy] != iy + 1) { - for (j = 0; j < 12; j++) { - temp = Y[iy + 9 * j]; - Y[iy + 9 * j] = Y[(ipiv[iy] + 9 * j) - 1]; - Y[(ipiv[iy] + 9 * j) - 1] = temp; + for (jA = 0; jA + 1 <= A_sizes[1]; jA++) { + if (ipiv_data[jA] != jA + 1) { + for (j = 0; j < 9; j++) { + temp = B_data[jA + B_sizes[0] * j]; + B_data[jA + B_sizes[0] * j] = B_data[(ipiv_data[jA] + B_sizes[0] * j) - + 1]; + B_data[(ipiv_data[jA] + B_sizes[0] * j) - 1] = temp; } } } - for (j = 0; j < 12; j++) { - c = 9 * j; - for (k = 0; k < 9; k++) { - jy = 9 * k; - if ((real_T)Y[k + c] != 0.0) { - for (iy = k + 2; iy < 10; iy++) { - Y[(iy + c) - 1] -= Y[k + c] * b_A[(iy + jy) - 1]; + if (B_sizes[0] == 0) { + } else { + for (j = 0; j < 9; j++) { + c = A_sizes[1] * j; + for (k = 0; k + 1 <= A_sizes[1]; k++) { + iy = A_sizes[1] * k; + if ((real_T)B_data[k + c] != 0.0) { + for (jA = k + 1; jA + 1 <= A_sizes[1]; jA++) { + B_data[jA + c] -= B_data[k + c] * b_A_data[jA + iy]; + } } } } } - for (j = 0; j < 12; j++) { - c = 9 * j; - for (k = 8; k > -1; k += -1) { - jy = 9 * k; - if ((real_T)Y[k + c] != 0.0) { - Y[k + c] /= b_A[k + jy]; - for (iy = 0; iy + 1 <= k; iy++) { - Y[iy + c] -= Y[k + c] * b_A[iy + jy]; + if (B_sizes[0] == 0) { + } else { + for (j = 0; j < 9; j++) { + c = A_sizes[1] * j; + for (k = A_sizes[1] - 1; k + 1 > 0; k--) { + iy = A_sizes[1] * k; + if ((real_T)B_data[k + c] != 0.0) { + B_data[k + c] /= b_A_data[k + iy]; + for (jA = 0; jA + 1 <= k; jA++) { + B_data[jA + c] -= B_data[k + c] * b_A_data[jA + iy]; + } + } + } + } + } +} + +/* + * + */ +static real32_T eml_matlab_zlarfg(int32_T n, real32_T *alpha1, real32_T x_data + [81], int32_T x_sizes[2], int32_T ix0) +{ + real32_T tau; + int32_T nm1; + real32_T xnorm; + real32_T a; + int32_T knt; + int32_T loop_ub; + int32_T k; + tau = 0.0F; + if (n <= 0) { + } else { + nm1 = n - 2; + xnorm = eml_xnrm2(nm1 + 1, x_data, x_sizes, ix0); + if ((real_T)xnorm != 0.0) { + a = (real32_T)fabs(*alpha1); + xnorm = (real32_T)fabs(xnorm); + if (a < xnorm) { + a /= xnorm; + xnorm *= (real32_T)sqrt(a * a + 1.0F); + } else if (a > xnorm) { + xnorm /= a; + xnorm = (real32_T)sqrt(xnorm * xnorm + 1.0F) * a; + } else if (rtIsNaNF(xnorm)) { + } else { + xnorm = a * 1.41421354F; + } + + if ((real_T)*alpha1 >= 0.0) { + xnorm = -xnorm; + } + + if ((real32_T)fabs(xnorm) < 9.86076132E-32F) { + knt = 0; + do { + knt++; + loop_ub = ix0 + nm1; + for (k = ix0; k <= loop_ub; k++) { + x_data[k - 1] *= 1.01412048E+31F; + } + + xnorm *= 1.01412048E+31F; + *alpha1 *= 1.01412048E+31F; + } while (!((real32_T)fabs(xnorm) >= 9.86076132E-32F)); + + xnorm = eml_xnrm2(nm1 + 1, x_data, x_sizes, ix0); + a = (real32_T)fabs(*alpha1); + xnorm = (real32_T)fabs(xnorm); + if (a < xnorm) { + a /= xnorm; + xnorm *= (real32_T)sqrt(a * a + 1.0F); + } else if (a > xnorm) { + xnorm /= a; + xnorm = (real32_T)sqrt(xnorm * xnorm + 1.0F) * a; + } else if (rtIsNaNF(xnorm)) { + } else { + xnorm = a * 1.41421354F; + } + + if ((real_T)*alpha1 >= 0.0) { + xnorm = -xnorm; + } + + tau = (xnorm - *alpha1) / xnorm; + *alpha1 = 1.0F / (*alpha1 - xnorm); + loop_ub = ix0 + nm1; + for (k = ix0; k <= loop_ub; k++) { + x_data[k - 1] *= *alpha1; + } + + for (k = 1; k <= knt; k++) { + xnorm *= 9.86076132E-32F; + } + + *alpha1 = xnorm; + } else { + tau = (xnorm - *alpha1) / xnorm; + *alpha1 = 1.0F / (*alpha1 - xnorm); + loop_ub = ix0 + nm1; + for (k = ix0; k <= loop_ub; k++) { + x_data[k - 1] *= *alpha1; + } + + *alpha1 = xnorm; + } + } + } + + return tau; +} + +/* + * + */ +static void eml_qrsolve(const real32_T A_data[81], const int32_T A_sizes[2], + real32_T B_data[81], int32_T B_sizes[2], real32_T Y_data[81], int32_T Y_sizes + [2]) +{ + real_T rankR; + real_T u1; + int32_T mn; + int32_T b_A_sizes[2]; + int32_T loop_ub; + int32_T k; + real32_T b_A_data[81]; + int32_T pvt; + int32_T b_mn; + int32_T tmp_data[9]; + int32_T jpvt_data[9]; + int8_T unnamed_idx_0; + real32_T work_data[9]; + int32_T nmi; + real32_T vn1_data[9]; + real32_T vn2_data[9]; + int32_T i; + int32_T i_i; + int32_T mmi; + int32_T ix; + real32_T smax; + real32_T s; + int32_T iy; + real32_T atmp; + real32_T tau_data[9]; + int32_T i_ip1; + int32_T lastv; + int32_T lastc; + boolean_T exitg2; + int32_T exitg1; + boolean_T firstNonZero; + real32_T t; + rankR = (real_T)A_sizes[0]; + u1 = (real_T)A_sizes[1]; + rankR = rankR <= u1 ? rankR : u1; + mn = (int32_T)rankR; + b_A_sizes[0] = A_sizes[0]; + b_A_sizes[1] = A_sizes[1]; + loop_ub = A_sizes[0] * A_sizes[1] - 1; + for (k = 0; k <= loop_ub; k++) { + b_A_data[k] = A_data[k]; + } + + k = A_sizes[0]; + pvt = A_sizes[1]; + b_mn = k <= pvt ? k : pvt; + if (A_sizes[1] < 1) { + } else { + loop_ub = A_sizes[1] - 1; + for (k = 0; k <= loop_ub; k++) { + tmp_data[k] = 1 + k; + } + + loop_ub = A_sizes[1] - 1; + for (k = 0; k <= loop_ub; k++) { + jpvt_data[k] = tmp_data[k]; + } + } + + if ((A_sizes[0] == 0) || (A_sizes[1] == 0)) { + } else { + unnamed_idx_0 = (int8_T)A_sizes[1]; + loop_ub = unnamed_idx_0 - 1; + for (k = 0; k <= loop_ub; k++) { + work_data[k] = 0.0F; + } + + k = 1; + for (nmi = 0; nmi + 1 <= A_sizes[1]; nmi++) { + vn1_data[nmi] = eml_xnrm2(A_sizes[0], A_data, A_sizes, k); + vn2_data[nmi] = vn1_data[nmi]; + k += A_sizes[0]; + } + + for (i = 0; i + 1 <= b_mn; i++) { + i_i = i + i * A_sizes[0]; + nmi = A_sizes[1] - i; + mmi = (A_sizes[0] - i) - 1; + if (nmi < 1) { + pvt = -1; + } else { + pvt = 0; + if (nmi > 1) { + ix = i; + smax = (real32_T)fabs(vn1_data[i]); + for (k = 1; k + 1 <= nmi; k++) { + ix++; + s = (real32_T)fabs(vn1_data[ix]); + if (s > smax) { + pvt = k; + smax = s; + } + } + } + } + + pvt += i; + if (pvt + 1 != i + 1) { + ix = A_sizes[0] * pvt; + iy = A_sizes[0] * i; + for (k = 1; k <= A_sizes[0]; k++) { + s = b_A_data[ix]; + b_A_data[ix] = b_A_data[iy]; + b_A_data[iy] = s; + ix++; + iy++; + } + + k = jpvt_data[pvt]; + jpvt_data[pvt] = jpvt_data[i]; + jpvt_data[i] = k; + vn1_data[pvt] = vn1_data[i]; + vn2_data[pvt] = vn2_data[i]; + } + + if (i + 1 < A_sizes[0]) { + atmp = b_A_data[i_i]; + smax = eml_matlab_zlarfg(mmi + 1, &atmp, b_A_data, b_A_sizes, i_i + 2); + tau_data[i] = smax; + } else { + atmp = b_A_data[i_i]; + smax = b_A_data[i_i]; + s = b_eml_matlab_zlarfg(&atmp, &smax); + b_A_data[i_i] = smax; + tau_data[i] = s; + } + + b_A_data[i_i] = atmp; + if (i + 1 < A_sizes[1]) { + atmp = b_A_data[i_i]; + b_A_data[i_i] = 1.0F; + i_ip1 = (i + (i + 1) * A_sizes[0]) + 1; + if ((real_T)tau_data[i] != 0.0) { + lastv = mmi; + pvt = i_i + mmi; + while ((lastv + 1 > 0) && ((real_T)b_A_data[pvt] == 0.0)) { + lastv--; + pvt--; + } + + lastc = nmi - 1; + exitg2 = 0U; + while ((exitg2 == 0U) && (lastc > 0)) { + k = i_ip1 + (lastc - 1) * A_sizes[0]; + pvt = k + lastv; + do { + exitg1 = 0U; + if (k <= pvt) { + if ((real_T)b_A_data[k - 1] != 0.0) { + exitg1 = 1U; + } else { + k++; + } + } else { + lastc--; + exitg1 = 2U; + } + } while (exitg1 == 0U); + + if (exitg1 == 1U) { + exitg2 = 1U; + } + } + } else { + lastv = -1; + lastc = 0; + } + + if (lastv + 1 > 0) { + if (lastc == 0) { + } else { + for (iy = 1; iy <= lastc; iy++) { + work_data[iy - 1] = 0.0F; + } + + iy = 0; + loop_ub = i_ip1 + A_sizes[0] * (lastc - 1); + pvt = i_ip1; + while ((A_sizes[0] > 0) && (pvt <= loop_ub)) { + ix = i_i; + smax = 0.0F; + k = pvt + lastv; + for (nmi = pvt; nmi <= k; nmi++) { + smax += b_A_data[nmi - 1] * b_A_data[ix]; + ix++; + } + + work_data[iy] += smax; + iy++; + pvt += A_sizes[0]; + } + } + + smax = -tau_data[i]; + if ((real_T)smax == 0.0) { + } else { + pvt = 0; + for (nmi = 1; nmi <= lastc; nmi++) { + if ((real_T)work_data[pvt] != 0.0) { + s = work_data[pvt] * smax; + ix = i_i; + loop_ub = lastv + i_ip1; + for (k = i_ip1 - 1; k + 1 <= loop_ub; k++) { + b_A_data[k] += b_A_data[ix] * s; + ix++; + } + } + + pvt++; + i_ip1 += A_sizes[0]; + } + } + } + + b_A_data[i_i] = atmp; + } + + for (nmi = i + 1; nmi + 1 <= A_sizes[1]; nmi++) { + if ((real_T)vn1_data[nmi] != 0.0) { + smax = (real32_T)fabs(b_A_data[i + b_A_sizes[0] * nmi]) / vn1_data[nmi]; + smax = 1.0F - smax * smax; + if ((real_T)smax < 0.0) { + smax = 0.0F; + } + + s = vn1_data[nmi] / vn2_data[nmi]; + if (smax * (s * s) <= 0.000345266977F) { + if (i + 1 < A_sizes[0]) { + k = (i + A_sizes[0] * nmi) + 1; + smax = 0.0F; + if (mmi < 1) { + } else if (mmi == 1) { + smax = (real32_T)fabs(b_A_data[k]); + } else { + s = 0.0F; + firstNonZero = TRUE; + pvt = k + mmi; + while (k + 1 <= pvt) { + if (b_A_data[k] != 0.0F) { + atmp = (real32_T)fabs(b_A_data[k]); + if (firstNonZero) { + s = atmp; + smax = 1.0F; + firstNonZero = FALSE; + } else if (s < atmp) { + t = s / atmp; + smax = 1.0F + smax * t * t; + s = atmp; + } else { + t = atmp / s; + smax += t * t; + } + } + + k++; + } + + smax = s * (real32_T)sqrt(smax); + } + + vn1_data[nmi] = smax; + vn2_data[nmi] = vn1_data[nmi]; + } else { + vn1_data[nmi] = 0.0F; + vn2_data[nmi] = 0.0F; + } + } else { + vn1_data[nmi] *= (real32_T)sqrt(smax); + } } } } } - for (jy = 0; jy < 9; jy++) { - for (iy = 0; iy < 12; iy++) { - y[iy + 12 * jy] = Y[jy + 9 * iy]; + rankR = (real_T)A_sizes[0]; + u1 = (real_T)A_sizes[1]; + rankR = rankR >= u1 ? rankR : u1; + smax = (real32_T)rankR * (real32_T)fabs(b_A_data[0]) * 1.1920929E-7F; + rankR = 0.0; + k = 0; + while ((k + 1 <= mn) && (!((real32_T)fabs(b_A_data[k + b_A_sizes[0] * k]) <= + smax))) { + rankR++; + k++; + } + + unnamed_idx_0 = (int8_T)A_sizes[1]; + Y_sizes[0] = (int32_T)unnamed_idx_0; + Y_sizes[1] = 9; + loop_ub = unnamed_idx_0 * 9 - 1; + for (k = 0; k <= loop_ub; k++) { + Y_data[k] = 0.0F; + } + + for (nmi = 0; nmi + 1 <= mn; nmi++) { + if ((real_T)tau_data[nmi] != 0.0) { + for (k = 0; k < 9; k++) { + smax = B_data[nmi + B_sizes[0] * k]; + for (i = nmi + 1; i + 1 <= A_sizes[0]; i++) { + smax += b_A_data[i + b_A_sizes[0] * nmi] * B_data[i + B_sizes[0] * k]; + } + + smax *= tau_data[nmi]; + if ((real_T)smax != 0.0) { + B_data[nmi + B_sizes[0] * k] -= smax; + for (i = nmi + 1; i + 1 <= A_sizes[0]; i++) { + B_data[i + B_sizes[0] * k] -= b_A_data[i + b_A_sizes[0] * nmi] * + smax; + } + } + } + } + } + + for (k = 0; k < 9; k++) { + for (i = 0; (real_T)(i + 1) <= rankR; i++) { + Y_data[(jpvt_data[i] + Y_sizes[0] * k) - 1] = B_data[i + B_sizes[0] * k]; + } + + for (nmi = (int32_T)rankR - 1; nmi + 1 >= 1; nmi--) { + Y_data[(jpvt_data[nmi] + Y_sizes[0] * k) - 1] /= b_A_data[nmi + b_A_sizes + [0] * nmi]; + for (i = 0; i + 1 <= nmi; i++) { + Y_data[(jpvt_data[i] + Y_sizes[0] * k) - 1] -= Y_data[(jpvt_data[nmi] + + Y_sizes[0] * k) - 1] * b_A_data[i + b_A_sizes[0] * nmi]; + } + } + } +} + +/* + * + */ +static real32_T eml_xnrm2(int32_T n, const real32_T x_data[81], const int32_T + x_sizes[2], int32_T ix0) +{ + real32_T y; + real32_T scale; + boolean_T firstNonZero; + int32_T kend; + int32_T k; + real32_T absxk; + real32_T t; + y = 0.0F; + if (n < 1) { + } else if (n == 1) { + y = (real32_T)fabs(x_data[ix0 - 1]); + } else { + scale = 0.0F; + firstNonZero = TRUE; + kend = (ix0 + n) - 1; + for (k = ix0 - 1; k + 1 <= kend; k++) { + if (x_data[k] != 0.0F) { + absxk = (real32_T)fabs(x_data[k]); + if (firstNonZero) { + scale = absxk; + y = 1.0F; + firstNonZero = FALSE; + } else if (scale < absxk) { + t = scale / absxk; + y = 1.0F + y * t * t; + scale = absxk; + } else { + t = absxk / scale; + y += t * t; + } + } + } + + y = scale * (real32_T)sqrt(y); + } + + return y; +} + +/* + * + */ +void mrdivide(const real32_T A_data[81], const int32_T A_sizes[2], const + real32_T B_data[81], const int32_T B_sizes[2], real32_T y_data[81], + int32_T y_sizes[2]) +{ + int32_T b_A_sizes[2]; + int32_T loop_ub; + int32_T i3; + int32_T b_loop_ub; + int32_T i4; + real32_T b_A_data[81]; + int32_T b_B_sizes[2]; + real32_T b_B_data[81]; + int8_T unnamed_idx_0; + int32_T c_B_sizes[2]; + real32_T c_B_data[81]; + b_A_sizes[0] = B_sizes[1]; + b_A_sizes[1] = B_sizes[0]; + loop_ub = B_sizes[0] - 1; + for (i3 = 0; i3 <= loop_ub; i3++) { + b_loop_ub = B_sizes[1] - 1; + for (i4 = 0; i4 <= b_loop_ub; i4++) { + b_A_data[i4 + b_A_sizes[0] * i3] = B_data[i3 + B_sizes[0] * i4]; + } + } + + b_B_sizes[0] = A_sizes[1]; + b_B_sizes[1] = 9; + for (i3 = 0; i3 < 9; i3++) { + loop_ub = A_sizes[1] - 1; + for (i4 = 0; i4 <= loop_ub; i4++) { + b_B_data[i4 + b_B_sizes[0] * i3] = A_data[i3 + A_sizes[0] * i4]; + } + } + + if ((b_A_sizes[0] == 0) || (b_A_sizes[1] == 0) || (b_B_sizes[0] == 0)) { + unnamed_idx_0 = (int8_T)b_A_sizes[1]; + b_B_sizes[0] = (int32_T)unnamed_idx_0; + b_B_sizes[1] = 9; + loop_ub = unnamed_idx_0 * 9 - 1; + for (i3 = 0; i3 <= loop_ub; i3++) { + b_B_data[i3] = 0.0F; + } + } else if (b_A_sizes[0] == b_A_sizes[1]) { + eml_lusolve(b_A_data, b_A_sizes, b_B_data, b_B_sizes); + } else { + c_B_sizes[0] = b_B_sizes[0]; + c_B_sizes[1] = 9; + loop_ub = b_B_sizes[0] * 9 - 1; + for (i3 = 0; i3 <= loop_ub; i3++) { + c_B_data[i3] = b_B_data[i3]; + } + + eml_qrsolve(b_A_data, b_A_sizes, c_B_data, c_B_sizes, b_B_data, b_B_sizes); + } + + y_sizes[0] = 9; + y_sizes[1] = b_B_sizes[0]; + loop_ub = b_B_sizes[0] - 1; + for (i3 = 0; i3 <= loop_ub; i3++) { + for (i4 = 0; i4 < 9; i4++) { + y_data[i4 + 9 * i3] = b_B_data[i3 + b_B_sizes[0] * i4]; } } } diff --git a/apps/attitude_estimator_ekf/codegen/mrdivide.h b/apps/attitude_estimator_ekf/codegen/mrdivide.h index e81bfffc0a..a58faaa262 100755 --- a/apps/attitude_estimator_ekf/codegen/mrdivide.h +++ b/apps/attitude_estimator_ekf/codegen/mrdivide.h @@ -3,7 +3,7 @@ * * Code generation for function 'mrdivide' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:23 2012 * */ @@ -14,6 +14,8 @@ #include #include #include +#include "rt_defines.h" +#include "rt_nonfinite.h" #include "rtwtypes.h" #include "attitudeKalmanfilter_types.h" @@ -27,6 +29,6 @@ /* Variable Definitions */ /* Function Declarations */ -extern void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108]); +extern void mrdivide(const real32_T A_data[81], const int32_T A_sizes[2], const real32_T B_data[81], const int32_T B_sizes[2], real32_T y_data[81], int32_T y_sizes[2]); #endif /* End of code generation (mrdivide.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/norm.c b/apps/attitude_estimator_ekf/codegen/norm.c index 1fbde052bc..8d8e4e1109 100755 --- a/apps/attitude_estimator_ekf/codegen/norm.c +++ b/apps/attitude_estimator_ekf/codegen/norm.c @@ -3,7 +3,7 @@ * * Code generation for function 'norm' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:23 2012 * */ @@ -40,7 +40,7 @@ real32_T norm(const real32_T x[3]) firstNonZero = TRUE; for (k = 0; k < 3; k++) { if (x[k] != 0.0F) { - absxk = fabsf(x[k]); + absxk = (real32_T)fabs(x[k]); if (firstNonZero) { scale = absxk; y = 1.0F; @@ -56,7 +56,7 @@ real32_T norm(const real32_T x[3]) } } - return scale * sqrtf(y); + return scale * (real32_T)sqrt(y); } /* End of code generation (norm.c) */ diff --git a/apps/attitude_estimator_ekf/codegen/norm.h b/apps/attitude_estimator_ekf/codegen/norm.h index b0c7fe430e..479503ae36 100755 --- a/apps/attitude_estimator_ekf/codegen/norm.h +++ b/apps/attitude_estimator_ekf/codegen/norm.h @@ -3,7 +3,7 @@ * * Code generation for function 'norm' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:23 2012 * */ @@ -14,6 +14,8 @@ #include #include #include +#include "rt_defines.h" +#include "rt_nonfinite.h" #include "rtwtypes.h" #include "attitudeKalmanfilter_types.h" diff --git a/apps/attitude_estimator_ekf/codegen/rtGetInf.c b/apps/attitude_estimator_ekf/codegen/rtGetInf.c index 3cef176848..42b3af501f 100755 --- a/apps/attitude_estimator_ekf/codegen/rtGetInf.c +++ b/apps/attitude_estimator_ekf/codegen/rtGetInf.c @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:24 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rtGetInf.h b/apps/attitude_estimator_ekf/codegen/rtGetInf.h index ab2d5a70d5..20d8c7f058 100755 --- a/apps/attitude_estimator_ekf/codegen/rtGetInf.h +++ b/apps/attitude_estimator_ekf/codegen/rtGetInf.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:24 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rtGetNaN.c b/apps/attitude_estimator_ekf/codegen/rtGetNaN.c index 17a093461f..d29aea34b6 100755 --- a/apps/attitude_estimator_ekf/codegen/rtGetNaN.c +++ b/apps/attitude_estimator_ekf/codegen/rtGetNaN.c @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:24 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rtGetNaN.h b/apps/attitude_estimator_ekf/codegen/rtGetNaN.h index 2c254bbbf6..a14b6170b8 100755 --- a/apps/attitude_estimator_ekf/codegen/rtGetNaN.h +++ b/apps/attitude_estimator_ekf/codegen/rtGetNaN.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:24 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rt_defines.h b/apps/attitude_estimator_ekf/codegen/rt_defines.h new file mode 100755 index 0000000000..4cad63ada3 --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/rt_defines.h @@ -0,0 +1,24 @@ +/* + * rt_defines.h + * + * Code generation for function 'attitudeKalmanfilter' + * + * C source code generated on: Mon Sep 17 20:13:24 2012 + * + */ + +#ifndef __RT_DEFINES_H__ +#define __RT_DEFINES_H__ + +#include + +#define RT_PI 3.14159265358979323846 +#define RT_PIF 3.1415927F +#define RT_LN_10 2.30258509299404568402 +#define RT_LN_10F 2.3025851F +#define RT_LOG10E 0.43429448190325182765 +#define RT_LOG10EF 0.43429449F +#define RT_E 2.7182818284590452354 +#define RT_EF 2.7182817F +#endif +/* End of code generation (rt_defines.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c index 005c4f28de..bd04f52e66 100755 --- a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c +++ b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:24 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h index 6481f011f8..67b3ba205d 100755 --- a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h +++ b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:24 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rtw_proj.tmw b/apps/attitude_estimator_ekf/codegen/rtw_proj.tmw deleted file mode 100755 index 1fb585abd7..0000000000 --- a/apps/attitude_estimator_ekf/codegen/rtw_proj.tmw +++ /dev/null @@ -1 +0,0 @@ -Code generation project for attitudeKalmanfilter using C:\Program Files\MATLAB\R2011a\toolbox\coder\coder\rtw\c\xrt\xrt_vcx64.tmf. MATLAB root = C:\Program Files\MATLAB\R2011a. diff --git a/apps/attitude_estimator_ekf/codegen/rtwtypes.h b/apps/attitude_estimator_ekf/codegen/rtwtypes.h index fd629ebcd0..6feb2e1a99 100755 --- a/apps/attitude_estimator_ekf/codegen/rtwtypes.h +++ b/apps/attitude_estimator_ekf/codegen/rtwtypes.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:24 2012 * */ From dbd6cbea60ade756b10c693b905fb3a85329e185 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 19 Sep 2012 22:43:00 +0200 Subject: [PATCH 03/24] 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/24] 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/24] 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/24] 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/24] 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/24] 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/24] 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/24] 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/24] 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/24] Untested, but fully implemented attitude and/or inner rate control --- .../multirotor_att_control_main.c | 130 ++++++++++++------ .../multirotor_attitude_control.c | 20 ++- .../multirotor_attitude_control.h | 5 +- 3 files changed, 106 insertions(+), 49 deletions(-) diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 5bc0d5fa3f..21c720096e 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -56,6 +56,7 @@ #include #include #include +#include #include #include #include @@ -72,16 +73,66 @@ __EXPORT int multirotor_att_control_main(int argc, char *argv[]); - -static enum { - CONTROL_MODE_RATES = 0, - CONTROL_MODE_ATTITUDE = 1, -} control_mode; - - static bool thread_should_exit; static int mc_task; static bool motor_test_mode = false; +static struct actuator_controls_s actuators; +static orb_advert_t actuator_pub; + +/** + * Perform rate control right after gyro reading + */ +static void *rate_control_thread_main(void *arg) +{ + prctl(PR_SET_NAME, "mc rate control", getpid()); + + int gyro_sub = orb_subscribe(ORB_ID(sensor_gyro)); + int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); + + struct pollfd fds = { .fd = gyro_sub, .events = POLLIN }; + + struct gyro_report gyro_report; + struct vehicle_rates_setpoint_s rates_sp; + memset(&rates_sp, 0, sizeof(rates_sp)); + float gyro_lp[3] = {0.0f, 0.0f, 0.0f}; + + while (!thread_should_exit) { + /* rate control at maximum rate */ + /* wait for a sensor update, check for exit condition every 1000 ms */ + int ret = poll(&fds, 1, 1000); + + if (ret < 0) { + /* XXX this is seriously bad - should be an emergency */ + } else if (ret == 0) { + /* XXX this means no sensor data - should be critical or emergency */ + printf("[mc att control] WARNING: Not getting gyro data, no rate control\n"); + } else { + /* get data */ + orb_copy(ORB_ID(sensor_gyro), gyro_sub, &gyro_report); + bool rates_sp_valid = false; + orb_check(rates_sp_sub, &rates_sp_valid); + if (rates_sp_valid) { + orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp); + } + + /* perform local lowpass */ + + /* apply controller */ + // if (state.flag_control_rates_enabled) { + /* lowpass gyros */ + // XXX + gyro_lp[0] = gyro_report.x; + gyro_lp[1] = gyro_report.y; + gyro_lp[2] = gyro_report.z; + + multirotor_control_rates(&rates_sp, gyro_lp, &actuators); + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + // } + } + } + + return NULL; +} static int mc_thread_main(int argc, char *argv[]) @@ -102,8 +153,6 @@ mc_thread_main(int argc, char *argv[]) struct vehicle_rates_setpoint_s rates_sp; memset(&rates_sp, 0, sizeof(rates_sp)); - struct actuator_controls_s actuators; - /* subscribe to attitude, motor setpoints and system state */ int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); @@ -127,8 +176,9 @@ mc_thread_main(int argc, char *argv[]) actuators.control[i] = 0.0f; } - orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); + actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); + orb_advert_t rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); /* register the perf counter */ perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control"); @@ -136,6 +186,13 @@ mc_thread_main(int argc, char *argv[]) /* welcome user */ printf("[multirotor_att_control] starting\n"); + /* ready, spawn pthread */ + pthread_attr_t rate_control_attr; + pthread_attr_init(&rate_control_attr); + pthread_attr_setstacksize(&rate_control_attr, 2048); + pthread_t rate_control_thread; + pthread_create(&rate_control_thread, &rate_control_attr, rate_control_thread_main, NULL); + while (!thread_should_exit) { /* wait for a sensor update, check for exit condition every 500 ms */ @@ -170,16 +227,21 @@ mc_thread_main(int argc, char *argv[]) att_sp.yaw_body = manual.yaw; // XXX Hack, remove, switch to yaw rate controller /* set yaw rate */ rates_sp.yaw = manual.yaw; + att_sp.thrust = manual.throttle; att_sp.timestamp = hrt_absolute_time(); + /* STEP 2: publish the result to the vehicle actuators */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); if (motor_test_mode) { att_sp.roll_body = 0.0f; att_sp.pitch_body = 0.0f; att_sp.yaw_body = 0.0f; att_sp.thrust = 0.1f; - } else { - att_sp.thrust = manual.throttle; + att_sp.timestamp = hrt_absolute_time(); + /* STEP 2: publish the result to the vehicle actuators */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); } + } else if (state.flag_control_offboard_enabled) { /* offboard inputs */ if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) { @@ -187,38 +249,32 @@ mc_thread_main(int argc, char *argv[]) rates_sp.pitch = offboard_sp.p2; rates_sp.yaw = offboard_sp.p3; rates_sp.thrust = offboard_sp.p4; + rates_sp.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); } else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) { att_sp.roll_body = offboard_sp.p1; att_sp.pitch_body = offboard_sp.p2; att_sp.yaw_body = offboard_sp.p3; att_sp.thrust = offboard_sp.p4; + att_sp.timestamp = hrt_absolute_time(); + /* STEP 2: publish the result to the vehicle actuators */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); } /* decide wether we want rate or position input */ } - /** STEP 2: Identify the controller setup to run and set up the inputs correctly */ + /** STEP 3: Identify the controller setup to run and set up the inputs correctly */ /* run attitude controller */ - if (state.flag_control_attitude_enabled) { - multirotor_control_attitude(&att_sp, &att, &actuators); + if (state.flag_control_attitude_enabled && !state.flag_control_rates_enabled) { + multirotor_control_attitude(&att_sp, &att, NULL, &actuators); + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + } else if (state.flag_control_attitude_enabled && state.flag_control_rates_enabled) { + multirotor_control_attitude(&att_sp, &att, &rates_sp, NULL); + orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); } - /* XXX could be also run in an independent loop */ - if (state.flag_control_rates_enabled) { - /* lowpass gyros */ - // XXX - static float gyro_lp[3] = {0, 0, 0}; - gyro_lp[0] = raw.gyro_rad_s[0]; - gyro_lp[1] = raw.gyro_rad_s[1]; - gyro_lp[2] = raw.gyro_rad_s[2]; - - multirotor_control_rates(&rates_sp, gyro_lp, &actuators); - } - - /* STEP 3: publish the result to the vehicle actuators */ - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); perf_end(mc_loop_perf); } @@ -240,6 +296,8 @@ mc_thread_main(int argc, char *argv[]) perf_print_counter(mc_loop_perf); perf_free(mc_loop_perf); + pthread_join(rate_control_thread, NULL); + fflush(stdout); exit(0); } @@ -258,22 +316,10 @@ usage(const char *reason) int multirotor_att_control_main(int argc, char *argv[]) { int ch; - - control_mode = CONTROL_MODE_RATES; unsigned int optioncount = 0; while ((ch = getopt(argc, argv, "tm:")) != EOF) { switch (ch) { - case 'm': - if (!strcmp(optarg, "rates")) { - control_mode = CONTROL_MODE_RATES; - } else if (!strcmp(optarg, "attitude")) { - control_mode = CONTROL_MODE_RATES; - } else { - usage("unrecognized -m value"); - } - optioncount += 2; - break; case 't': motor_test_mode = true; optioncount += 1; diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c index 6e2a544389..2129915d12 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -188,7 +188,7 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc } void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, - const struct vehicle_attitude_s *att, struct actuator_controls_s *actuators) + const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, struct actuator_controls_s *actuators) { static uint64_t last_run = 0; const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; @@ -305,10 +305,20 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s roll_controller.saturated = 1; } - actuators->control[0] = roll_control; - actuators->control[1] = pitch_control; - actuators->control[2] = yaw_rate_control; - actuators->control[3] = motor_thrust; + if (actuators) { + actuators->control[0] = roll_control; + actuators->control[1] = pitch_control; + actuators->control[2] = yaw_rate_control; + actuators->control[3] = motor_thrust; + } + + // XXX change yaw rate to yaw pos controller + if (rates_sp) { + rates_sp->roll = roll_control; + rates_sp->pitch = pitch_control; + rates_sp->yaw = yaw_rate_control; + rates_sp->thrust = motor_thrust; + } motor_skip_counter++; } diff --git a/apps/multirotor_att_control/multirotor_attitude_control.h b/apps/multirotor_att_control/multirotor_attitude_control.h index aa8d27369b..d9d3c04447 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.h +++ b/apps/multirotor_att_control/multirotor_attitude_control.h @@ -48,9 +48,10 @@ #include #include #include +#include #include -void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, - struct actuator_controls_s *actuators); +void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, + const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, struct actuator_controls_s *actuators); #endif /* MULTIROTOR_ATTITUDE_CONTROL_H_ */ From 6b0ed71ae02a56692a80e47bd11baa7e14d9284e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 25 Sep 2012 16:36:33 +0200 Subject: [PATCH 13/24] Simplified magnetometer calibration routine --- apps/commander/commander.c | 179 +++++++++-------------------------- apps/systemlib/param/param.c | 4 +- 2 files changed, 49 insertions(+), 134 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 7535b90469..4c20a2f71e 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -307,30 +307,10 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) const uint64_t calibration_interval_us = 45 * 1000000; unsigned int calibration_counter = 0; - const int peak_samples = 2000; - /* Get rid of 10% */ - const int outlier_margin = (peak_samples) / 10; + const int peak_samples = 500; - float *mag_maxima[3]; - mag_maxima[0] = (float*)malloc(peak_samples * sizeof(float)); - mag_maxima[1] = (float*)malloc(peak_samples * sizeof(float)); - mag_maxima[2] = (float*)malloc(peak_samples * sizeof(float)); - - float *mag_minima[3]; - mag_minima[0] = (float*)malloc(peak_samples * sizeof(float)); - mag_minima[1] = (float*)malloc(peak_samples * sizeof(float)); - mag_minima[2] = (float*)malloc(peak_samples * sizeof(float)); - - /* initialize data table */ - for (int i = 0; i < peak_samples; i++) { - mag_maxima[0][i] = FLT_MIN; - mag_maxima[1][i] = FLT_MIN; - mag_maxima[2][i] = FLT_MIN; - - mag_minima[0][i] = FLT_MAX; - mag_minima[1][i] = FLT_MAX; - mag_minima[2][i] = FLT_MAX; - } + float mag_max[3] = {0, 0, 0}; + float mag_min[3] = {0, 0, 0}; int fd = open(MAG_DEVICE_PATH, 0); struct mag_scale mscale_null = { @@ -357,27 +337,25 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); /* get min/max values */ - /* iterate through full list */ - for (int i = 0; i < peak_samples; i++) { - /* x minimum */ - if (raw.magnetometer_raw[0] < mag_minima[0][i]) - mag_minima[0][i] = raw.magnetometer_ga[0]; - /* y minimum */ - if (raw.magnetometer_raw[1] < mag_minima[1][i]) - mag_minima[1][i] = raw.magnetometer_ga[1]; - /* z minimum */ - if (raw.magnetometer_raw[2] < mag_minima[2][i]) - mag_minima[2][i] = raw.magnetometer_ga[2]; + if (raw.magnetometer_raw[0] < mag_min[0]) { + mag_min[0] = raw.magnetometer_raw[0]; + } + else if (raw.magnetometer_raw[0] > mag_max[0]) { + mag_max[0] = raw.magnetometer_raw[0]; + } - /* x maximum */ - if (raw.magnetometer_raw[0] > mag_maxima[0][i]) - mag_maxima[0][i] = raw.magnetometer_ga[0]; - /* y maximum */ - if (raw.magnetometer_raw[1] > mag_maxima[1][i]) - mag_maxima[1][i] = raw.magnetometer_ga[1]; - /* z maximum */ - if (raw.magnetometer_raw[2] > mag_maxima[2][i]) - mag_maxima[2][i] = raw.magnetometer_ga[2]; + if (raw.magnetometer_raw[1] < mag_min[1]) { + mag_min[1] = raw.magnetometer_raw[1]; + } + else if (raw.magnetometer_raw[1] > mag_max[1]) { + mag_max[1] = raw.magnetometer_raw[1]; + } + + if (raw.magnetometer_raw[2] < mag_min[2]) { + mag_min[2] = raw.magnetometer_raw[2]; + } + else if (raw.magnetometer_raw[2] > mag_max[2]) { + mag_max[2] = raw.magnetometer_raw[2]; } calibration_counter++; @@ -392,67 +370,6 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) status->flag_preflight_mag_calibration = false; state_machine_publish(status_pub, status, mavlink_fd); - /* sort values */ - cal_bsort(mag_minima[0], peak_samples); - cal_bsort(mag_minima[1], peak_samples); - cal_bsort(mag_minima[2], peak_samples); - - cal_bsort(mag_maxima[0], peak_samples); - cal_bsort(mag_maxima[1], peak_samples); - cal_bsort(mag_maxima[2], peak_samples); - - float min_avg[3] = { 0.0f, 0.0f, 0.0f }; - float max_avg[3] = { 0.0f, 0.0f, 0.0f }; - - // printf("start:\n"); - - // for (int i = 0; i < 10; i++) { - // printf("mag min: %8.4f\t%8.4f\t%8.4f\tmax: %8.4f\t%8.4f\t%8.4f\n", - // mag_minima[0][i], - // mag_minima[1][i], - // mag_minima[2][i], - // mag_maxima[0][i], - // mag_maxima[1][i], - // mag_maxima[2][i]); - // usleep(10000); - // } - // printf("-----\n"); - - // for (int i = (peak_samples - outlier_margin)-10; i < (peak_samples - outlier_margin); i++) { - // printf("mag min: %8.4f\t%8.4f\t%8.4f\tmax: %8.4f\t%8.4f\t%8.4f\n", - // mag_minima[0][i], - // mag_minima[1][i], - // mag_minima[2][i], - // mag_maxima[0][i], - // mag_maxima[1][i], - // mag_maxima[2][i]); - // usleep(10000); - // } - - // printf("end\n"); - - /* take average of center value group */ - for (int i = 0; i < (peak_samples - outlier_margin); i++) { - min_avg[0] += mag_minima[0][i+outlier_margin]; - min_avg[1] += mag_minima[1][i+outlier_margin]; - min_avg[2] += mag_minima[2][i+outlier_margin]; - - max_avg[0] += mag_maxima[0][i]; - max_avg[1] += mag_maxima[1][i]; - max_avg[2] += mag_maxima[2][i]; - } - - min_avg[0] /= (peak_samples - outlier_margin); - min_avg[1] /= (peak_samples - outlier_margin); - min_avg[2] /= (peak_samples - outlier_margin); - - max_avg[0] /= (peak_samples - outlier_margin); - max_avg[1] /= (peak_samples - outlier_margin); - max_avg[2] /= (peak_samples - outlier_margin); - - // printf("\nFINAL:\nmag min: %8.4f\t%8.4f\t%8.4f\nmax: %8.4f\t%8.4f\t%8.4f\n", (double)min_avg[0], - // (double)min_avg[1], (double)min_avg[2], (double)max_avg[0], (double)max_avg[1], (double)max_avg[2]); - float mag_offset[3]; /** @@ -467,31 +384,37 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) * offset = (max + min) / 2.0f */ - mag_offset[0] = (max_avg[0] + min_avg[0]) / 2.0f; - mag_offset[1] = (max_avg[1] + min_avg[1]) / 2.0f; - mag_offset[2] = (max_avg[2] + min_avg[2]) / 2.0f; + printf("max 0: %f\n",mag_max[0]); + printf("max 1: %f\n",mag_max[1]); + printf("max 2: %f\n",mag_max[2]); + printf("min 0: %f\n",mag_min[0]); + printf("min 1: %f\n",mag_min[1]); + printf("min 2: %f\n",mag_min[2]); - if (!isfinite(mag_offset[1]) || !isfinite(mag_offset[1]) || !isfinite(mag_offset[2])) { + mag_offset[0] = (mag_max[0] + mag_min[0]) / 2.0f; + mag_offset[1] = (mag_max[1] + mag_min[1]) / 2.0f; + mag_offset[2] = (mag_max[2] + mag_min[2]) / 2.0f; - mavlink_log_critical(mavlink_fd, "[commander] MAG calibration failed (INF/NAN)"); - } else { - /* announce and set new offset */ + printf("mag off 0: %f\n",mag_offset[0]); + printf("mag off 1: %f\n",mag_offset[1]); + printf("mag off 2: %f\n",mag_offset[2]); - // char offset_output[50]; - // sprintf(offset_output, "[commander] mag cal: %8.4f %8.4f %8.4f", (double)mag_offset[0], (double)mag_offset[1], (double)mag_offset[2]); - // mavlink_log_info(mavlink_fd, offset_output); + /* announce and set new offset */ - if (param_set(param_find("SENS_MAG_XOFF"), &(mag_offset[0]))) { - fprintf(stderr, "[commander] Setting X mag offset failed!\n"); - } - - if (param_set(param_find("SENS_MAG_YOFF"), &(mag_offset[1]))) { - fprintf(stderr, "[commander] Setting Y mag offset failed!\n"); - } + // char offset_output[50]; + // sprintf(offset_output, "[commander] mag cal: %8.4f %8.4f %8.4f", (double)mag_offset[0], (double)mag_offset[1], (double)mag_offset[2]); + // mavlink_log_info(mavlink_fd, offset_output); - if (param_set(param_find("SENS_MAG_ZOFF"), &(mag_offset[2]))) { - fprintf(stderr, "[commander] Setting Z mag offset failed!\n"); - } + if (param_set(param_find("SENS_MAG_XOFF"), &(mag_offset[0]))) { + fprintf(stderr, "[commander] Setting X mag offset failed!\n"); + } + + if (param_set(param_find("SENS_MAG_YOFF"), &(mag_offset[1]))) { + fprintf(stderr, "[commander] Setting Y mag offset failed!\n"); + } + + if (param_set(param_find("SENS_MAG_ZOFF"), &(mag_offset[2]))) { + fprintf(stderr, "[commander] Setting Z mag offset failed!\n"); } fd = open(MAG_DEVICE_PATH, 0); @@ -507,14 +430,6 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) warn("WARNING: failed to set scale / offsets for mag"); close(fd); - free(mag_maxima[0]); - free(mag_maxima[1]); - free(mag_maxima[2]); - - free(mag_minima[0]); - free(mag_minima[1]); - free(mag_minima[2]); - /* auto-save to EEPROM */ int save_ret = pm_save_eeprom(false); if(save_ret != 0) { diff --git a/apps/systemlib/param/param.c b/apps/systemlib/param/param.c index a01c170a68..0ab7c0ea30 100644 --- a/apps/systemlib/param/param.c +++ b/apps/systemlib/param/param.c @@ -484,7 +484,7 @@ param_export(int fd, bool only_unsaved) s->unsaved = false; - /* append the appripriate BSON type object */ + /* append the appropriate BSON type object */ switch (param_type(s->param)) { case PARAM_TYPE_INT32: param_get(s->param, &i); @@ -688,4 +688,4 @@ param_foreach(void (*func)(void *arg, param_t param), void *arg, bool only_chang func(arg, param); } -} \ No newline at end of file +} From 5c00ca343fabce95273d15b13da4460935134fd3 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 25 Sep 2012 16:39:15 +0200 Subject: [PATCH 14/24] forgot to remove printfs of magnetometer calibration --- apps/commander/commander.c | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 4c20a2f71e..3ac01ee1bd 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -384,20 +384,20 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) * offset = (max + min) / 2.0f */ - printf("max 0: %f\n",mag_max[0]); - printf("max 1: %f\n",mag_max[1]); - printf("max 2: %f\n",mag_max[2]); - printf("min 0: %f\n",mag_min[0]); - printf("min 1: %f\n",mag_min[1]); - printf("min 2: %f\n",mag_min[2]); +// printf("max 0: %f\n",mag_max[0]); +// printf("max 1: %f\n",mag_max[1]); +// printf("max 2: %f\n",mag_max[2]); +// printf("min 0: %f\n",mag_min[0]); +// printf("min 1: %f\n",mag_min[1]); +// printf("min 2: %f\n",mag_min[2]); mag_offset[0] = (mag_max[0] + mag_min[0]) / 2.0f; mag_offset[1] = (mag_max[1] + mag_min[1]) / 2.0f; mag_offset[2] = (mag_max[2] + mag_min[2]) / 2.0f; - printf("mag off 0: %f\n",mag_offset[0]); - printf("mag off 1: %f\n",mag_offset[1]); - printf("mag off 2: %f\n",mag_offset[2]); +// printf("mag off 0: %f\n",mag_offset[0]); +// printf("mag off 1: %f\n",mag_offset[1]); +// printf("mag off 2: %f\n",mag_offset[2]); /* announce and set new offset */ From abbe998506e4ba49bbf6a9a9ae731b1eec521db6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 25 Sep 2012 21:35:02 +0200 Subject: [PATCH 15/24] ardrone in the air again (workaround: rate controller disabled) --- apps/ardrone_interface/ardrone_interface.c | 11 +++++++++-- apps/ardrone_interface/ardrone_motor_control.c | 9 ++++++--- .../multirotor_att_control_main.c | 16 +++++++++++++--- .../multirotor_attitude_control.c | 7 +++++++ 4 files changed, 35 insertions(+), 8 deletions(-) diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c index 8d77e7502e..f12f9cb474 100644 --- a/apps/ardrone_interface/ardrone_interface.c +++ b/apps/ardrone_interface/ardrone_interface.c @@ -239,14 +239,15 @@ int ardrone_interface_thread_main(int argc, char *argv[]) /* declare and safely initialize all structs */ struct vehicle_status_s state; - memset(&state, 0, sizeof(state)); + //memset(&state, 0, sizeof(state)); struct actuator_controls_s actuator_controls; - memset(&actuator_controls, 0, sizeof(actuator_controls)); + //memset(&actuator_controls, 0, sizeof(actuator_controls)); struct actuator_armed_s armed; armed.armed = false; /* subscribe to attitude, motor setpoints and system state */ int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + int state_sub = orb_subscribe(ORB_ID(vehicle_status)); int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); @@ -328,7 +329,13 @@ int ardrone_interface_thread_main(int argc, char *argv[]) * if in failsafe */ if (armed.armed && !armed.lockdown) { + + + + //printf("AMO_BEF: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",actuator_controls.control[0], actuator_controls.control[1], actuator_controls.control[2], actuator_controls.control[3]); + ardrone_mixing_and_output(ardrone_write, &actuator_controls); + } else { /* Silently lock down motor speeds to zero */ ardrone_write_motor_commands(ardrone_write, 0, 0, 0, 0); diff --git a/apps/ardrone_interface/ardrone_motor_control.c b/apps/ardrone_interface/ardrone_motor_control.c index 787db18773..cbf9600a59 100644 --- a/apps/ardrone_interface/ardrone_motor_control.c +++ b/apps/ardrone_interface/ardrone_motor_control.c @@ -368,6 +368,8 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls float yaw_control = actuators->control[2]; float motor_thrust = actuators->control[3]; + //printf("AMO: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",roll_control, pitch_control, yaw_control, motor_thrust); + const float min_thrust = 0.02f; /**< 2% minimum thrust */ const float max_thrust = 1.0f; /**< 100% max thrust */ const float scaling = 512.0f; /**< 100% thrust equals a value of 512 */ @@ -387,15 +389,16 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls if (motor_thrust <= min_thrust) { motor_thrust = min_thrust; output_band = 0.0f; - + //printf("0 silent\n"); } else if (motor_thrust < startpoint_full_control && motor_thrust > min_thrust) { output_band = band_factor * (motor_thrust - min_thrust); - + //printf("1 starting\n"); } else if (motor_thrust >= startpoint_full_control && motor_thrust < max_thrust - band_factor * startpoint_full_control) { output_band = band_factor * startpoint_full_control; - + //printf("2 working\n"); } else if (motor_thrust >= max_thrust - band_factor * startpoint_full_control) { output_band = band_factor * (max_thrust - motor_thrust); + //printf("3 full\n"); } //add the yaw, nick and roll components to the basic thrust //TODO:this should be done by the mixer diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 21c720096e..70b9d8e28d 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -76,7 +76,7 @@ __EXPORT int multirotor_att_control_main(int argc, char *argv[]); static bool thread_should_exit; static int mc_task; static bool motor_test_mode = false; -static struct actuator_controls_s actuators; + static orb_advert_t actuator_pub; /** @@ -86,6 +86,8 @@ static void *rate_control_thread_main(void *arg) { prctl(PR_SET_NAME, "mc rate control", getpid()); + struct actuator_controls_s actuators; + int gyro_sub = orb_subscribe(ORB_ID(sensor_gyro)); int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); @@ -125,8 +127,8 @@ static void *rate_control_thread_main(void *arg) gyro_lp[1] = gyro_report.y; gyro_lp[2] = gyro_report.z; - multirotor_control_rates(&rates_sp, gyro_lp, &actuators); - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); +// multirotor_control_rates(&rates_sp, gyro_lp, &actuators); +// orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); // } } } @@ -153,6 +155,8 @@ mc_thread_main(int argc, char *argv[]) struct vehicle_rates_setpoint_s rates_sp; memset(&rates_sp, 0, sizeof(rates_sp)); + struct actuator_controls_s actuators; + /* subscribe to attitude, motor setpoints and system state */ int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); @@ -228,6 +232,7 @@ mc_thread_main(int argc, char *argv[]) /* set yaw rate */ rates_sp.yaw = manual.yaw; att_sp.thrust = manual.throttle; + //printf("Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",att_sp.roll_body, att_sp.pitch_body, att_sp.yaw_body, att_sp.thrust); att_sp.timestamp = hrt_absolute_time(); /* STEP 2: publish the result to the vehicle actuators */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); @@ -269,10 +274,15 @@ mc_thread_main(int argc, char *argv[]) /* run attitude controller */ if (state.flag_control_attitude_enabled && !state.flag_control_rates_enabled) { multirotor_control_attitude(&att_sp, &att, NULL, &actuators); +// printf("publish actuator\n"); + +// printf("MAC_PUB: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",actuators.control[0], actuators.control[1], actuators.control[2], actuators.control[3]); + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); } else if (state.flag_control_attitude_enabled && state.flag_control_rates_enabled) { multirotor_control_attitude(&att_sp, &att, &rates_sp, NULL); orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); +// printf("publish attitude\n"); } diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c index 2129915d12..c25e96856a 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -312,6 +312,10 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s actuators->control[3] = motor_thrust; } +// if(motor_skip_counter%20 == 0) +// printf("MAC: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",actuators->control[0], actuators->control[1], actuators->control[2], actuators->control[3]); + + // XXX change yaw rate to yaw pos controller if (rates_sp) { rates_sp->roll = roll_control; @@ -320,5 +324,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s rates_sp->thrust = motor_thrust; } +// if(motor_skip_counter%20 == 0) +// printf("Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",rates_sp->roll, rates_sp->pitch, rates_sp->yaw, rates_sp->thrust); + motor_skip_counter++; } From 201fdbc42c46bc9146a8cbf2434a98792d6d9f50 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 26 Sep 2012 10:11:57 +0200 Subject: [PATCH 16/24] ardrone flying now (still workaround of disabled rates controller) --- .../multirotor_att_control_main.c | 15 ++++++++------- apps/sensors/sensors.cpp | 4 +++- 2 files changed, 11 insertions(+), 8 deletions(-) diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 70b9d8e28d..904872dde2 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -127,8 +127,8 @@ static void *rate_control_thread_main(void *arg) gyro_lp[1] = gyro_report.y; gyro_lp[2] = gyro_report.z; -// multirotor_control_rates(&rates_sp, gyro_lp, &actuators); -// orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + multirotor_control_rates(&rates_sp, gyro_lp, &actuators); + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); // } } } @@ -192,10 +192,10 @@ mc_thread_main(int argc, char *argv[]) /* ready, spawn pthread */ pthread_attr_t rate_control_attr; - pthread_attr_init(&rate_control_attr); - pthread_attr_setstacksize(&rate_control_attr, 2048); - pthread_t rate_control_thread; - pthread_create(&rate_control_thread, &rate_control_attr, rate_control_thread_main, NULL); +// pthread_attr_init(&rate_control_attr); +// pthread_attr_setstacksize(&rate_control_attr, 2048); +// pthread_t rate_control_thread; +// pthread_create(&rate_control_thread, &rate_control_attr, rate_control_thread_main, NULL); while (!thread_should_exit) { @@ -306,7 +306,7 @@ mc_thread_main(int argc, char *argv[]) perf_print_counter(mc_loop_perf); perf_free(mc_loop_perf); - pthread_join(rate_control_thread, NULL); + //pthread_join(rate_control_thread, NULL); fflush(stdout); exit(0); @@ -340,6 +340,7 @@ int multirotor_att_control_main(int argc, char *argv[]) default: fprintf(stderr, "option: -%c\n", ch); usage("unrecognized option"); + break; } } argc -= optioncount; diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index eb22ac8a70..ceb8c4b104 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -968,7 +968,7 @@ Sensors::ppm_poll() _rc.chan[i].scaled = (ppm_buffer[i] - _parameters.trim[i]) / (float)(_parameters.max[i] - _parameters.trim[i]); } else if (ppm_buffer[i] < (_parameters.trim[i] - _parameters.dz[i])) { /* division by zero impossible for trim == min (as for throttle), as this falls in the above if clause */ - _rc.chan[i].scaled = -1.0f + ((ppm_buffer[i] - _parameters.min[i]) / (float)(_parameters.trim[i] - _parameters.min[i])); + _rc.chan[i].scaled = -((_parameters.trim[i] - ppm_buffer[i]) / (float)(_parameters.trim[i] - _parameters.min[i])); } else { /* in the configured dead zone, output zero */ @@ -1029,6 +1029,8 @@ Sensors::ppm_poll() orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc); orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control); + +// printf("SENSORS: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",manual_control.roll, manual_control.pitch, manual_control.yaw, manual_control.throttle); } #endif From ac43a67a0ff8be62504e3398def6b1f6f0719e14 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 26 Sep 2012 14:29:47 +0200 Subject: [PATCH 17/24] ardrone max motor output was slightly to high --- apps/ardrone_interface/ardrone_motor_control.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/ardrone_interface/ardrone_motor_control.c b/apps/ardrone_interface/ardrone_motor_control.c index cbf9600a59..89ed183ccd 100644 --- a/apps/ardrone_interface/ardrone_motor_control.c +++ b/apps/ardrone_interface/ardrone_motor_control.c @@ -372,7 +372,7 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls const float min_thrust = 0.02f; /**< 2% minimum thrust */ const float max_thrust = 1.0f; /**< 100% max thrust */ - const float scaling = 512.0f; /**< 100% thrust equals a value of 512 */ + const float scaling = 500.0f; /**< 100% thrust equals a value of 500 which works, 512 leads to cutoff */ const float min_gas = min_thrust * scaling; /**< value range sent to motors, minimum */ const float max_gas = max_thrust * scaling; /**< value range sent to motors, maximum */ From cbb1f1c9eda7f4eb3b4b8be8be80eeb1ad7b9209 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 26 Sep 2012 21:30:03 +0200 Subject: [PATCH 18/24] Fixed RC and offboard control state machine --- apps/commander/commander.c | 57 ++++++++++++++++++------------- apps/uORB/topics/vehicle_status.h | 1 + 2 files changed, 35 insertions(+), 23 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 4be004ad62..e653ef72f8 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1135,6 +1135,8 @@ int commander_thread_main(int argc, char *argv[]) memset(¤t_status, 0, sizeof(current_status)); current_status.state_machine = SYSTEM_STATE_PREFLIGHT; current_status.flag_system_armed = false; + current_status.offboard_control_signal_found_once = false; + current_status.rc_signal_found_once = false; /* advertise to ORB */ stat_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); @@ -1214,8 +1216,16 @@ int commander_thread_main(int argc, char *argv[]) while (!thread_should_exit) { /* Get current values */ - orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man); - orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); + bool new_data; + orb_check(sp_man_sub, &new_data); + if (new_data) { + orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man); + } + + orb_check(sp_offboard_sub, &new_data); + if (new_data) { + orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); + } orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps); orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors); @@ -1385,9 +1395,8 @@ int commander_thread_main(int argc, char *argv[]) /* ignore RC signals if in offboard control mode */ - if (!current_status.offboard_control_signal_found_once) { + if (!current_status.offboard_control_signal_found_once && sp_man.timestamp != 0) { /* Start RC state check */ - bool prev_lost = current_status.rc_signal_lost; if ((hrt_absolute_time() - sp_man.timestamp) < 100000) { @@ -1440,38 +1449,33 @@ int commander_thread_main(int argc, char *argv[]) if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[commander] RECOVERY - RC SIGNAL GAINED!"); } + current_status.rc_signal_cutting_off = false; current_status.rc_signal_lost = false; current_status.rc_signal_lost_interval = 0; } else { static uint64_t last_print_time = 0; /* print error message for first RC glitch and then every 5 s / 5000 ms) */ - if (!current_status.rc_signal_lost || ((hrt_absolute_time() - last_print_time) > 5000000)) { + if (!current_status.rc_signal_cutting_off || ((hrt_absolute_time() - last_print_time) > 5000000)) { /* only complain if the offboard control is NOT active */ + current_status.rc_signal_cutting_off = true; mavlink_log_critical(mavlink_fd, "[commander] CRITICAL - NO REMOTE SIGNAL!"); last_print_time = hrt_absolute_time(); } /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ - current_status.rc_signal_cutting_off = true; current_status.rc_signal_lost_interval = hrt_absolute_time() - sp_man.timestamp; /* if the RC signal is gone for a full second, consider it lost */ if (current_status.rc_signal_lost_interval > 1000000) { current_status.rc_signal_lost = true; current_status.failsave_lowlevel = true; + state_changed = true; } // if (hrt_absolute_time() - current_status.failsave_ll_start_time > failsafe_lowlevel_timeout_ms*1000) { // publish_armed_status(¤t_status); // } } - - /* Check if this is the first loss or first gain*/ - if ((!prev_lost && current_status.rc_signal_lost) || - (prev_lost && !current_status.rc_signal_lost)) { - /* publish change */ - publish_armed_status(¤t_status); - } } @@ -1483,23 +1487,29 @@ int commander_thread_main(int argc, char *argv[]) /* State machine update for offboard control */ - if (!current_status.rc_signal_found_once) { + if (!current_status.rc_signal_found_once && sp_offboard.timestamp != 0) { if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) { + /* decide about attitude control flag, enable in att/pos/vel */ + bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE || + sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || + sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION); + + /* decide about rate control flag, enable it always XXX (for now) */ + bool rates_ctrl_enabled = true; + /* set up control mode */ - if (current_status.flag_control_attitude_enabled != - (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE)) { - current_status.flag_control_attitude_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE); + if (current_status.flag_control_attitude_enabled != attitude_ctrl_enabled) { + current_status.flag_control_attitude_enabled = attitude_ctrl_enabled; state_changed = true; } - if (current_status.flag_control_rates_enabled != - (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES)) { - current_status.flag_control_attitude_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES); + if (current_status.flag_control_rates_enabled != rates_ctrl_enabled) { + current_status.flag_control_rates_enabled = rates_ctrl_enabled; state_changed = true; } - /* handle the case where RC signal was regained */ + /* handle the case where offboard control signal was regained */ if (!current_status.offboard_control_signal_found_once) { current_status.offboard_control_signal_found_once = true; /* enable offboard control, disable manual input */ @@ -1515,6 +1525,7 @@ int commander_thread_main(int argc, char *argv[]) } } + current_status.offboard_control_signal_weak = false; current_status.offboard_control_signal_lost = false; current_status.offboard_control_signal_lost_interval = 0; @@ -1530,8 +1541,8 @@ int commander_thread_main(int argc, char *argv[]) } else { static uint64_t last_print_time = 0; /* print error message for first RC glitch and then every 5 s / 5000 ms) */ - if (!current_status.offboard_control_signal_lost || ((hrt_absolute_time() - last_print_time) > 5000000)) { - /* only complain if the RC control is NOT active */ + if (!current_status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_time) > 5000000)) { + current_status.offboard_control_signal_weak = true; mavlink_log_critical(mavlink_fd, "[commander] CRITICAL - NO OFFBOARD CONTROL SIGNAL!"); last_print_time = hrt_absolute_time(); } diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index c727527b16..23172d7cf1 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -143,6 +143,7 @@ struct vehicle_status_s bool offboard_control_signal_found_once; bool offboard_control_signal_lost; + bool offboard_control_signal_weak; uint64_t offboard_control_signal_lost_interval; /**< interval in microseconds without an offboard control message */ bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */ From d7456e61ffdf8587973e977a529d297aed233c22 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 26 Sep 2012 22:16:57 +0200 Subject: [PATCH 19/24] Fixed a max value in the AR.Drone interface --- apps/ardrone_interface/ardrone_interface.c | 2 +- apps/ardrone_interface/ardrone_motor_control.c | 4 ++-- apps/ardrone_interface/ardrone_motor_control.h | 3 +++ 3 files changed, 6 insertions(+), 3 deletions(-) diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c index 8d77e7502e..8031497d2f 100644 --- a/apps/ardrone_interface/ardrone_interface.c +++ b/apps/ardrone_interface/ardrone_interface.c @@ -1,7 +1,7 @@ /**************************************************************************** * * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier + * Author: Lorenz Meier * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/apps/ardrone_interface/ardrone_motor_control.c b/apps/ardrone_interface/ardrone_motor_control.c index 787db18773..edb518631d 100644 --- a/apps/ardrone_interface/ardrone_motor_control.c +++ b/apps/ardrone_interface/ardrone_motor_control.c @@ -306,7 +306,7 @@ int ar_init_motors(int ardrone_uart, int gpios) return errcounter; } -/* +/** * Sets the leds on the motor controllers, 1 turns led on, 0 off. */ void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green, uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green) @@ -370,7 +370,7 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls const float min_thrust = 0.02f; /**< 2% minimum thrust */ const float max_thrust = 1.0f; /**< 100% max thrust */ - const float scaling = 512.0f; /**< 100% thrust equals a value of 512 */ + const float scaling = 500.0f; /**< 100% thrust equals a value of 500 which works, 512 leads to cutoff */ const float min_gas = min_thrust * scaling; /**< value range sent to motors, minimum */ const float max_gas = max_thrust * scaling; /**< value range sent to motors, maximum */ diff --git a/apps/ardrone_interface/ardrone_motor_control.h b/apps/ardrone_interface/ardrone_motor_control.h index 664419707a..78b603b63a 100644 --- a/apps/ardrone_interface/ardrone_motor_control.h +++ b/apps/ardrone_interface/ardrone_motor_control.h @@ -87,4 +87,7 @@ int ar_init_motors(int ardrone_uart, int gpio); */ void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green, uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green); +/** + * Mix motors and output actuators + */ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls_s *actuators); From f93464e64fa3bc9d8b5113c67aa690e5bc56d065 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 26 Sep 2012 22:17:13 +0200 Subject: [PATCH 20/24] Fixed RC scaling in sensors app --- apps/sensors/sensors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index eb22ac8a70..3aaef422c9 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -968,7 +968,7 @@ Sensors::ppm_poll() _rc.chan[i].scaled = (ppm_buffer[i] - _parameters.trim[i]) / (float)(_parameters.max[i] - _parameters.trim[i]); } else if (ppm_buffer[i] < (_parameters.trim[i] - _parameters.dz[i])) { /* division by zero impossible for trim == min (as for throttle), as this falls in the above if clause */ - _rc.chan[i].scaled = -1.0f + ((ppm_buffer[i] - _parameters.min[i]) / (float)(_parameters.trim[i] - _parameters.min[i])); + _rc.chan[i].scaled = -((_parameters.trim[i] - ppm_buffer[i]) / (float)(_parameters.trim[i] - _parameters.min[i])); } else { /* in the configured dead zone, output zero */ From ec3949bf82dbaa50ea866b65cd0fc4630af18001 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 26 Sep 2012 22:25:39 +0200 Subject: [PATCH 21/24] Fix a bug where the rate controller is always active --- .../multirotor_att_control_main.c | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 21c720096e..9cb3831b12 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -79,6 +79,8 @@ static bool motor_test_mode = false; static struct actuator_controls_s actuators; static orb_advert_t actuator_pub; +static struct vehicle_status_s state; + /** * Perform rate control right after gyro reading */ @@ -118,7 +120,7 @@ static void *rate_control_thread_main(void *arg) /* perform local lowpass */ /* apply controller */ - // if (state.flag_control_rates_enabled) { + if (state.flag_control_rates_enabled) { /* lowpass gyros */ // XXX gyro_lp[0] = gyro_report.x; @@ -127,7 +129,7 @@ static void *rate_control_thread_main(void *arg) multirotor_control_rates(&rates_sp, gyro_lp, &actuators); orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - // } + } } } @@ -138,7 +140,6 @@ static int mc_thread_main(int argc, char *argv[]) { /* declare and safely initialize all structs */ - struct vehicle_status_s state; memset(&state, 0, sizeof(state)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); @@ -201,7 +202,11 @@ mc_thread_main(int argc, char *argv[]) perf_begin(mc_loop_perf); /* get a local copy of system state */ - orb_copy(ORB_ID(vehicle_status), state_sub, &state); + bool updated; + orb_check(state_sub, &updated); + if (updated) { + orb_copy(ORB_ID(vehicle_status), state_sub, &state); + } /* get a local copy of manual setpoint */ orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); /* get a local copy of attitude */ @@ -209,7 +214,6 @@ mc_thread_main(int argc, char *argv[]) /* get a local copy of attitude setpoint */ orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp); /* get a local copy of rates setpoint */ - bool updated; orb_check(setpoint_sub, &updated); if (updated) { orb_copy(ORB_ID(offboard_control_setpoint), setpoint_sub, &offboard_sp); From 7f153098926bf977609c6efb53fa7cb5093564af Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 27 Sep 2012 16:50:20 +0200 Subject: [PATCH 22/24] Calibration should not freeze anymore, ardrone flying but estimator is not able to use calibrated magnetometer data --- apps/ardrone_interface/ardrone_interface.c | 3 +- apps/commander/commander.c | 48 +++++++------------ .../multirotor_att_control_main.c | 10 ++-- 3 files changed, 23 insertions(+), 38 deletions(-) diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c index f0bc0b1e78..8b4b5c400f 100644 --- a/apps/ardrone_interface/ardrone_interface.c +++ b/apps/ardrone_interface/ardrone_interface.c @@ -140,9 +140,8 @@ static int ardrone_open_uart(char *uart_name, struct termios *uart_config_origin int speed = B115200; int uart; - /* open uart */ - printf("[ardrone_interface] UART is %s, baud rate is%d\n",uart_name,speed); + //printf("[ardrone_interface] UART is %s, baud rate is%d\n",uart_name,speed); uart = open(uart_name, O_RDWR | O_NOCTTY); /* Try to set baud rate */ diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 66a981a685..a1c2a15d26 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -312,8 +312,6 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) const uint64_t calibration_interval_us = 45 * 1000000; unsigned int calibration_counter = 0; - const int peak_samples = 500; - float mag_max[3] = {0, 0, 0}; float mag_min[3] = {0, 0, 0}; @@ -366,8 +364,8 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) calibration_counter++; } else { /* any poll failure for 1s is a reason to abort */ - //mavlink_log_info(mavlink_fd, "[commander] mag calibration aborted, please retry."); - //break; + mavlink_log_info(mavlink_fd, "[commander] mag calibration aborted, please retry."); + break; } } @@ -389,20 +387,20 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) * offset = (max + min) / 2.0f */ -// printf("max 0: %f\n",mag_max[0]); -// printf("max 1: %f\n",mag_max[1]); -// printf("max 2: %f\n",mag_max[2]); -// printf("min 0: %f\n",mag_min[0]); -// printf("min 1: %f\n",mag_min[1]); -// printf("min 2: %f\n",mag_min[2]); + printf("max 0: %f\n",mag_max[0]); + printf("max 1: %f\n",mag_max[1]); + printf("max 2: %f\n",mag_max[2]); + printf("min 0: %f\n",mag_min[0]); + printf("min 1: %f\n",mag_min[1]); + printf("min 2: %f\n",mag_min[2]); mag_offset[0] = (mag_max[0] + mag_min[0]) / 2.0f; mag_offset[1] = (mag_max[1] + mag_min[1]) / 2.0f; mag_offset[2] = (mag_max[2] + mag_min[2]) / 2.0f; -// printf("mag off 0: %f\n",mag_offset[0]); -// printf("mag off 1: %f\n",mag_offset[1]); -// printf("mag off 2: %f\n",mag_offset[2]); + printf("mag off 0: %f\n",mag_offset[0]); + printf("mag off 1: %f\n",mag_offset[1]); + printf("mag off 2: %f\n",mag_offset[2]); /* announce and set new offset */ @@ -487,7 +485,7 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) calibration_counter++; } else { /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "[commander] gyro calibration aborted, please retry."); + mavlink_log_info(mavlink_fd, "[commander] gyro calibration aborted, retry"); return; } } @@ -534,9 +532,7 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) mavlink_log_info(mavlink_fd, "[commander] gyro calibration finished"); - // char offset_output[50]; - // sprintf(offset_output, "[commander] gyro cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]); - // mavlink_log_info(mavlink_fd, offset_output); + printf("[commander] gyro cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]); close(sub_sensor_combined); } @@ -544,9 +540,8 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) void do_accel_calibration(int status_pub, struct vehicle_status_s *status) { /* announce change */ - usleep(5000); - mavlink_log_info(mavlink_fd, "[commander] The system should be level and not moved"); + mavlink_log_info(mavlink_fd, "[commander] keep it level and still"); /* set to accel calibration mode */ status->flag_preflight_accel_calibration = true; state_machine_publish(status_pub, status, mavlink_fd); @@ -571,7 +566,6 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale_null)) warn("WARNING: failed to set scale / offsets for accel"); close(fd); - while (calibration_counter < calibration_count) { /* wait blocking for new data */ @@ -585,11 +579,10 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) calibration_counter++; } else { /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "[commander] gyro calibration aborted, please retry."); + mavlink_log_info(mavlink_fd, "[commander] acceleration calibration aborted"); return; } } - accel_offset[0] = accel_offset[0] / calibration_count; accel_offset[1] = accel_offset[1] / calibration_count; accel_offset[2] = accel_offset[2] / calibration_count; @@ -644,18 +637,11 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) if(save_ret != 0) { warn("WARNING: auto-save of params to EEPROM failed"); } - /* exit to gyro calibration mode */ status->flag_preflight_accel_calibration = false; state_machine_publish(status_pub, status, mavlink_fd); - mavlink_log_info(mavlink_fd, "[commander] acceleration calibration finished"); - - // char offset_output[50]; - // sprintf(offset_output, "[commander] accel cal: x:%8.4f y:%8.4f z:%8.4f", (double)accel_offset[0], - // (double)accel_offset[1], (double)accel_offset[2]); - // mavlink_log_info(mavlink_fd, offset_output); - + printf("[commander] accel calibration: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0],(double)accel_offset[1], (double)accel_offset[2]); close(sub_sensor_combined); } @@ -823,7 +809,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* none found */ if (!handled) { //fprintf(stderr, "[commander] refusing unsupported calibration request\n"); - mavlink_log_critical(mavlink_fd, "[commander] CMD refusing unsupported calibration request"); + mavlink_log_critical(mavlink_fd, "[commander] CMD refusing unsup. calib. request"); result = MAV_RESULT_UNSUPPORTED; } } diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 8d648c1944..b00b6bc0ca 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -193,10 +193,10 @@ mc_thread_main(int argc, char *argv[]) /* ready, spawn pthread */ pthread_attr_t rate_control_attr; -// pthread_attr_init(&rate_control_attr); -// pthread_attr_setstacksize(&rate_control_attr, 2048); -// pthread_t rate_control_thread; -// pthread_create(&rate_control_thread, &rate_control_attr, rate_control_thread_main, NULL); + pthread_attr_init(&rate_control_attr); + pthread_attr_setstacksize(&rate_control_attr, 2048); + pthread_t rate_control_thread; + pthread_create(&rate_control_thread, &rate_control_attr, rate_control_thread_main, NULL); while (!thread_should_exit) { @@ -310,7 +310,7 @@ mc_thread_main(int argc, char *argv[]) perf_print_counter(mc_loop_perf); perf_free(mc_loop_perf); - //pthread_join(rate_control_thread, NULL); + pthread_join(rate_control_thread, NULL); fflush(stdout); exit(0); From 2c5c3141057be1f46cb4a33f71e2331ce36b18a7 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 27 Sep 2012 17:08:29 +0200 Subject: [PATCH 23/24] Cleanup of lots of debugging printfs --- apps/ardrone_interface/ardrone_interface.c | 11 ++--------- apps/ardrone_interface/ardrone_motor_control.c | 4 ---- apps/commander/commander.c | 4 ---- .../multirotor_att_control_main.c | 6 ------ .../multirotor_attitude_control.c | 7 ------- apps/sensors/sensors.cpp | 1 - 6 files changed, 2 insertions(+), 31 deletions(-) diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c index 8b4b5c400f..8ed6db6642 100644 --- a/apps/ardrone_interface/ardrone_interface.c +++ b/apps/ardrone_interface/ardrone_interface.c @@ -141,7 +141,6 @@ static int ardrone_open_uart(char *uart_name, struct termios *uart_config_origin int uart; /* open uart */ - //printf("[ardrone_interface] UART is %s, baud rate is%d\n",uart_name,speed); uart = open(uart_name, O_RDWR | O_NOCTTY); /* Try to set baud rate */ @@ -238,15 +237,14 @@ int ardrone_interface_thread_main(int argc, char *argv[]) /* declare and safely initialize all structs */ struct vehicle_status_s state; - //memset(&state, 0, sizeof(state)); + memset(&state, 0, sizeof(state)); struct actuator_controls_s actuator_controls; - //memset(&actuator_controls, 0, sizeof(actuator_controls)); + memset(&actuator_controls, 0, sizeof(actuator_controls)); struct actuator_armed_s armed; armed.armed = false; /* subscribe to attitude, motor setpoints and system state */ int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); - int state_sub = orb_subscribe(ORB_ID(vehicle_status)); int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); @@ -328,11 +326,6 @@ int ardrone_interface_thread_main(int argc, char *argv[]) * if in failsafe */ if (armed.armed && !armed.lockdown) { - - - - //printf("AMO_BEF: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",actuator_controls.control[0], actuator_controls.control[1], actuator_controls.control[2], actuator_controls.control[3]); - ardrone_mixing_and_output(ardrone_write, &actuator_controls); } else { diff --git a/apps/ardrone_interface/ardrone_motor_control.c b/apps/ardrone_interface/ardrone_motor_control.c index e410d3a71a..c68a26df96 100644 --- a/apps/ardrone_interface/ardrone_motor_control.c +++ b/apps/ardrone_interface/ardrone_motor_control.c @@ -388,16 +388,12 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls if (motor_thrust <= min_thrust) { motor_thrust = min_thrust; output_band = 0.0f; - //printf("0 silent\n"); } else if (motor_thrust < startpoint_full_control && motor_thrust > min_thrust) { output_band = band_factor * (motor_thrust - min_thrust); - //printf("1 starting\n"); } else if (motor_thrust >= startpoint_full_control && motor_thrust < max_thrust - band_factor * startpoint_full_control) { output_band = band_factor * startpoint_full_control; - //printf("2 working\n"); } else if (motor_thrust >= max_thrust - band_factor * startpoint_full_control) { output_band = band_factor * (max_thrust - motor_thrust); - //printf("3 full\n"); } //add the yaw, nick and roll components to the basic thrust //TODO:this should be done by the mixer diff --git a/apps/commander/commander.c b/apps/commander/commander.c index a1c2a15d26..1c23c1f9d2 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -404,10 +404,6 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) /* announce and set new offset */ - // char offset_output[50]; - // sprintf(offset_output, "[commander] mag cal: %8.4f %8.4f %8.4f", (double)mag_offset[0], (double)mag_offset[1], (double)mag_offset[2]); - // mavlink_log_info(mavlink_fd, offset_output); - if (param_set(param_find("SENS_MAG_XOFF"), &(mag_offset[0]))) { fprintf(stderr, "[commander] Setting X mag offset failed!\n"); } diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index b00b6bc0ca..ebd9911a36 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -236,7 +236,6 @@ mc_thread_main(int argc, char *argv[]) /* set yaw rate */ rates_sp.yaw = manual.yaw; att_sp.thrust = manual.throttle; - //printf("Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",att_sp.roll_body, att_sp.pitch_body, att_sp.yaw_body, att_sp.thrust); att_sp.timestamp = hrt_absolute_time(); /* STEP 2: publish the result to the vehicle actuators */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); @@ -278,15 +277,10 @@ mc_thread_main(int argc, char *argv[]) /* run attitude controller */ if (state.flag_control_attitude_enabled && !state.flag_control_rates_enabled) { multirotor_control_attitude(&att_sp, &att, NULL, &actuators); -// printf("publish actuator\n"); - -// printf("MAC_PUB: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",actuators.control[0], actuators.control[1], actuators.control[2], actuators.control[3]); - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); } else if (state.flag_control_attitude_enabled && state.flag_control_rates_enabled) { multirotor_control_attitude(&att_sp, &att, &rates_sp, NULL); orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); -// printf("publish attitude\n"); } diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c index c25e96856a..2129915d12 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -312,10 +312,6 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s actuators->control[3] = motor_thrust; } -// if(motor_skip_counter%20 == 0) -// printf("MAC: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",actuators->control[0], actuators->control[1], actuators->control[2], actuators->control[3]); - - // XXX change yaw rate to yaw pos controller if (rates_sp) { rates_sp->roll = roll_control; @@ -324,8 +320,5 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s rates_sp->thrust = motor_thrust; } -// if(motor_skip_counter%20 == 0) -// printf("Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",rates_sp->roll, rates_sp->pitch, rates_sp->yaw, rates_sp->thrust); - motor_skip_counter++; } diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index ceb8c4b104..f81dfa9b8f 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -1030,7 +1030,6 @@ Sensors::ppm_poll() orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc); orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control); -// printf("SENSORS: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",manual_control.roll, manual_control.pitch, manual_control.yaw, manual_control.throttle); } #endif From d206327541f159ac4abd76e66bce3160e8704231 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 27 Sep 2012 18:43:04 +0200 Subject: [PATCH 24/24] Magnetometer calibration fixed --- apps/commander/commander.c | 55 +++++++++++--------------------------- 1 file changed, 15 insertions(+), 40 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 1c23c1f9d2..960c5d3837 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -256,22 +256,6 @@ int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, u return 0; } -static void cal_bsort(float a[], int n) -{ - int i,j,t; - for(i=0;ia[j+1]) { - t=a[j]; - a[j]=a[j+1]; - a[j+1]=t; - } - } - } -} - static const char *parameter_file = "/eeprom/parameters"; static int pm_save_eeprom(bool only_unsaved) @@ -312,8 +296,8 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) const uint64_t calibration_interval_us = 45 * 1000000; unsigned int calibration_counter = 0; - float mag_max[3] = {0, 0, 0}; - float mag_min[3] = {0, 0, 0}; + float mag_max[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX}; + float mag_min[3] = {FLT_MAX, FLT_MAX, FLT_MAX}; int fd = open(MAG_DEVICE_PATH, 0); struct mag_scale mscale_null = { @@ -340,25 +324,25 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); /* get min/max values */ - if (raw.magnetometer_raw[0] < mag_min[0]) { - mag_min[0] = raw.magnetometer_raw[0]; + if (raw.magnetometer_ga[0] < mag_min[0]) { + mag_min[0] = raw.magnetometer_ga[0]; } - else if (raw.magnetometer_raw[0] > mag_max[0]) { - mag_max[0] = raw.magnetometer_raw[0]; + else if (raw.magnetometer_ga[0] > mag_max[0]) { + mag_max[0] = raw.magnetometer_ga[0]; } - if (raw.magnetometer_raw[1] < mag_min[1]) { - mag_min[1] = raw.magnetometer_raw[1]; + if (raw.magnetometer_ga[1] < mag_min[1]) { + mag_min[1] = raw.magnetometer_ga[1]; } - else if (raw.magnetometer_raw[1] > mag_max[1]) { - mag_max[1] = raw.magnetometer_raw[1]; + else if (raw.magnetometer_ga[1] > mag_max[1]) { + mag_max[1] = raw.magnetometer_ga[1]; } - if (raw.magnetometer_raw[2] < mag_min[2]) { - mag_min[2] = raw.magnetometer_raw[2]; + if (raw.magnetometer_ga[2] < mag_min[2]) { + mag_min[2] = raw.magnetometer_ga[2]; } - else if (raw.magnetometer_raw[2] > mag_max[2]) { - mag_max[2] = raw.magnetometer_raw[2]; + else if (raw.magnetometer_ga[2] > mag_max[2]) { + mag_max[2] = raw.magnetometer_ga[2]; } calibration_counter++; @@ -387,20 +371,11 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) * offset = (max + min) / 2.0f */ - printf("max 0: %f\n",mag_max[0]); - printf("max 1: %f\n",mag_max[1]); - printf("max 2: %f\n",mag_max[2]); - printf("min 0: %f\n",mag_min[0]); - printf("min 1: %f\n",mag_min[1]); - printf("min 2: %f\n",mag_min[2]); - mag_offset[0] = (mag_max[0] + mag_min[0]) / 2.0f; mag_offset[1] = (mag_max[1] + mag_min[1]) / 2.0f; mag_offset[2] = (mag_max[2] + mag_min[2]) / 2.0f; - printf("mag off 0: %f\n",mag_offset[0]); - printf("mag off 1: %f\n",mag_offset[1]); - printf("mag off 2: %f\n",mag_offset[2]); + printf("mag off x: %4.4f, y: %4.4f, z: %4.4f\n",(double)mag_offset[0],(double)mag_offset[0],(double)mag_offset[2]); /* announce and set new offset */