Skip to content

Commit

Permalink
simple_flight refactoring tested
Browse files Browse the repository at this point in the history
  • Loading branch information
@sytelus
sytelus committed Jul 31, 2017
1 parent 456b47a commit 3da637b
Show file tree
Hide file tree
Showing 23 changed files with 127 additions and 84 deletions.
6 changes: 3 additions & 3 deletions AirLib/AirLib.vcxproj
View file
Original file line number Diff line number Diff line change
Expand Up @@ -245,7 +245,7 @@
<ClCompile>
<PrecompiledHeader>
</PrecompiledHeader>
<WarningLevel>Level3</WarningLevel>
<WarningLevel>Level4</WarningLevel>
<Optimization>Disabled</Optimization>
<PreprocessorDefinitions>WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
<AdditionalIncludeDirectories>include;deps\eigen3;deps\rpclib\include;$(ProjectDir)..\MavLinkCom\include</AdditionalIncludeDirectories>
Expand Down Expand Up @@ -292,7 +292,7 @@
</ItemDefinitionGroup>
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
<ClCompile>
<WarningLevel>Level3</WarningLevel>
<WarningLevel>Level4</WarningLevel>
<PrecompiledHeader>
</PrecompiledHeader>
<Optimization>MaxSpeed</Optimization>
Expand Down Expand Up @@ -320,7 +320,7 @@
</ItemDefinitionGroup>
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
<ClCompile>
<WarningLevel>Level3</WarningLevel>
<WarningLevel>Level4</WarningLevel>
<PrecompiledHeader>
</PrecompiledHeader>
<Optimization>MaxSpeed</Optimization>
Expand Down
45 changes: 0 additions & 45 deletions AirLib/include/common/Common.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -69,50 +69,5 @@ inline std::ostream& operator<<(std::ostream &os, Vector3r const &vec) {
return os << vec.x() << "\t" << vec.y() << "\t" << vec.z() << "\t";
}

static void logWarning(const char* format, ...)
{
va_list args;
va_start(args, format);

IGNORE_FORMAT_STRING_ON
auto size = _vscprintf(format, args) + 1U;
IGNORE_FORMAT_STRING_OFF
std::unique_ptr<char[]> buf(new char[size] );

#ifndef _MSC_VER
IGNORE_FORMAT_STRING_ON
vsnprintf(buf.get(), size, format, args);
IGNORE_FORMAT_STRING_OFF
#else
vsnprintf_s(buf.get(), size, _TRUNCATE, format, args);
#endif

va_end(args);

std::cout << "WARNING: " << string(buf.get());
}
static void logError(const char* format, ...)
{
va_list args;
va_start(args, format);
IGNORE_FORMAT_STRING_ON
auto size = _vscprintf(format, args) + 1U;
IGNORE_FORMAT_STRING_OFF
std::unique_ptr<char[]> buf(new char[size] );

#ifndef _MSC_VER
IGNORE_FORMAT_STRING_ON
vsnprintf(buf.get(), size, format, args);
IGNORE_FORMAT_STRING_OFF
#else
vsnprintf_s(buf.get(), size, _TRUNCATE, format, args);
#endif

va_end(args);

std::cerr << "ERROR: " << string(buf.get());
}


}} //namespace
#endif
2 changes: 1 addition & 1 deletion AirLib/include/common/common_utils/StrictMode.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@
//'=': conversion from 'double' to 'float', possible loss of data
#define STRICT_MODE_OFF \
__pragma(warning(push)) \
__pragma(warning( disable : 4100 4189 4244 4245 4239 4267 4464 4456 4505 4514 4571 4624 4626 4668 4710 4820 5027 5031))
__pragma(warning( disable : 4100 4189 4244 4245 4239 4267 4365 4464 4456 4505 4514 4571 4624 4625 4626 4668 4710 4820 4917 5026 5027 5031))
#define STRICT_MODE_ON \
__pragma(warning(pop))

Expand Down
34 changes: 34 additions & 0 deletions AirLib/include/controllers/ros_flight/RosFlightDroneController.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -107,6 +107,7 @@ class RosFlightDroneController : public DroneControllerBase {
virtual void setOffboardMode(bool is_set) override
{
//TODO: implement this
unused(is_set);
}

virtual void setSimulationMode(bool is_set) override
Expand Down Expand Up @@ -170,26 +171,37 @@ class RosFlightDroneController : public DroneControllerBase {

bool armDisarm(bool arm, CancelableBase& cancelable_action) override
{
unused(arm);
unused(cancelable_action);

return true;
}

bool takeoff(float max_wait_seconds, CancelableBase& cancelable_action) override
{
unused(max_wait_seconds);
unused(cancelable_action);

return true;
}

bool land(float max_wait_seconds, CancelableBase& cancelable_action) override
{
unused(max_wait_seconds);
unused(cancelable_action);

return true;
}

bool goHome(CancelableBase& cancelable_action) override
{
unused(cancelable_action);
return true;
}

bool hover(CancelableBase& cancelable_action) override
{
unused(cancelable_action);
return true;
}

Expand All @@ -205,6 +217,8 @@ class RosFlightDroneController : public DroneControllerBase {

virtual void reportTelemetry(float renderTime) override
{
unused(renderTime);

//TODO: implement this
}

Expand All @@ -228,21 +242,41 @@ class RosFlightDroneController : public DroneControllerBase {
protected:
void commandRollPitchZ(float pitch, float roll, float z, float yaw) override
{
unused(pitch);
unused(roll);
unused(z);
unused(yaw);

//TODO: implement this
}

void commandVelocity(float vx, float vy, float vz, const YawMode& yaw_mode) override
{
unused(vx);
unused(vy);
unused(vz);
unused(yaw_mode);

//TODO: implement this
}

void commandVelocityZ(float vx, float vy, float z, const YawMode& yaw_mode) override
{
unused(vx);
unused(vy);
unused(z);
unused(yaw_mode);

//TODO: implement this
}

void commandPosition(float x, float y, float z, const YawMode& yaw_mode) override
{
unused(x);
unused(y);
unused(z);
unused(yaw_mode);

//TODO: implement this
}

Expand Down
4 changes: 2 additions & 2 deletions AirLib/include/controllers/simple_flight/AirSimSimpleFlightBoard.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -55,12 +55,12 @@ class AirSimSimpleFlightBoard : public simple_flight::IBoard {
return clock()->nowNanos() / 1000000;
}

virtual float readChannel(uint8_t index) const override
virtual float readChannel(uint16_t index) const override
{
return input_channels_[index];
}

virtual void writeOutput(uint8_t index, float value) override
virtual void writeOutput(uint16_t index, float value) override
{
motor_output_[index] = value;
}
Expand Down
4 changes: 2 additions & 2 deletions AirLib/include/controllers/simple_flight/AirSimSimpleFlightCommLink.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,9 +32,9 @@ class AirSimSimpleFlightCommLink : public simple_flight::ICommLink {
{
}

virtual void log(const char* message, int32_t error_level)
virtual void log(const std::string& message, int32_t log_level)
{
unused(error_level);
unused(log_level);
messages_.push_back(std::string(message));
}

Expand Down
4 changes: 2 additions & 2 deletions AirLib/include/controllers/simple_flight/AirSimSimpleFlightEstimator.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,9 +19,9 @@ class AirSimSimpleFlightEstimator : public simple_flight::IStateEstimator {
kinematics_ = kinematics;
}

virtual simple_flight::Angles getAngles() const override
virtual simple_flight::Axis3r getAngles() const override
{
simple_flight::Angles angles;
simple_flight::Axis3r angles;
VectorMath::toEulerianAngle(kinematics_->pose.orientation,
angles.pitch(), angles.roll(), angles.yaw());

Expand Down
30 changes: 30 additions & 0 deletions AirLib/include/controllers/simple_flight/SimpleFlightDroneController.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -97,6 +97,7 @@ class SimpleFlightDroneController : public DroneControllerBase {

virtual void setOffboardMode(bool is_set) override
{
unused(is_set);
//TODO: implement this
}

Expand Down Expand Up @@ -161,26 +162,34 @@ class SimpleFlightDroneController : public DroneControllerBase {

bool armDisarm(bool arm, CancelableBase& cancelable_action) override
{
unused(arm);
unused(cancelable_action);
return true;
}

bool takeoff(float max_wait_seconds, CancelableBase& cancelable_action) override
{
unused(max_wait_seconds);
unused(cancelable_action);
return true;
}

bool land(float max_wait_seconds, CancelableBase& cancelable_action) override
{
unused(max_wait_seconds);
unused(cancelable_action);
return true;
}

bool goHome(CancelableBase& cancelable_action) override
{
unused(cancelable_action);
return true;
}

bool hover(CancelableBase& cancelable_action) override
{
unused(cancelable_action);
return true;
}

Expand All @@ -196,6 +205,7 @@ class SimpleFlightDroneController : public DroneControllerBase {

virtual void reportTelemetry(float renderTime) override
{
unused(renderTime);
//TODO: implement this
}

Expand All @@ -219,21 +229,41 @@ class SimpleFlightDroneController : public DroneControllerBase {
protected:
void commandRollPitchZ(float pitch, float roll, float z, float yaw) override
{
unused(pitch);
unused(roll);
unused(z);
unused(yaw);

//TODO: implement this
}

void commandVelocity(float vx, float vy, float vz, const YawMode& yaw_mode) override
{
unused(vx);
unused(vy);
unused(vz);
unused(yaw_mode);

//TODO: implement this
}

void commandVelocityZ(float vx, float vy, float z, const YawMode& yaw_mode) override
{
unused(vx);
unused(vy);
unused(z);
unused(yaw_mode);

//TODO: implement this
}

void commandPosition(float x, float y, float z, const YawMode& yaw_mode) override
{
unused(x);
unused(y);
unused(z);
unused(yaw_mode);

//TODO: implement this
}

Expand Down
4 changes: 3 additions & 1 deletion AirLib/include/controllers/simple_flight/firmware/AngleLevelController.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@ class AngleLevelController :
rate_controller_->initialize(axis, this, state_estimator_);

//we will be setting goal for rate controller so we need these two things
rate_mode_ = GoalMode::getUnknow();
rate_mode_ = GoalMode::getUnknown();
rate_mode_[axis] = GoalModeType::AngleRate;

AngleLevelController::reset();
Expand All @@ -48,6 +48,8 @@ class AngleLevelController :
{
pid_->reset();
rate_controller_->reset();
rate_goal_ = Axis4r();
output_ = TReal();
}

virtual void update() override
Expand Down
1 change: 1 addition & 0 deletions AirLib/include/controllers/simple_flight/firmware/AngleRateController.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,7 @@ class AngleRateController : public IAxisController {
virtual void reset() override
{
pid_->reset();
output_ = TReal();
}

virtual void update() override
Expand Down
Loading

0 comments on commit 3da637b

Please sign in to comment.