zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

navigator / mission item: Compress fields into bitfield

sbg
Lorenz Meier 9 years ago committed by Lorenz Meier
parent
5899ce715d
commit
9821499113
8 changed files with 21 additions and 38 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 15
      src/modules/mavlink/mavlink_mission.cpp
  2. 2
      src/modules/navigator/datalinkloss.cpp
  3. 1
      src/modules/navigator/enginefailure.cpp
  4. 1
      src/modules/navigator/mission.cpp
  5. 16
      src/modules/navigator/mission_block.cpp
  6. 19
      src/modules/navigator/navigation.h
  7. 1
      src/modules/navigator/rcloss.cpp
  8. 4
      src/modules/navigator/rtl.cpp

15
src/modules/mavlink/mavlink_mission.cpp
Unescape View File

@ -912,16 +912,14 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
case MAV_CMD_NAV_LOITER_UNLIM: case MAV_CMD_NAV_LOITER_UNLIM:
mission_item->nav_cmd = NAV_CMD_LOITER_UNLIMITED; mission_item->nav_cmd = NAV_CMD_LOITER_UNLIMITED;
mission_item->loiter_radius = fabsf(mavlink_mission_item->param3); mission_item->loiter_radius = mavlink_mission_item->param3;
mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */
mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F); mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F);
break; break;
case MAV_CMD_NAV_LOITER_TIME: case MAV_CMD_NAV_LOITER_TIME:
mission_item->nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; mission_item->nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
mission_item->time_inside = mavlink_mission_item->param1; mission_item->time_inside = mavlink_mission_item->param1;
mission_item->loiter_radius = fabsf(mavlink_mission_item->param3); mission_item->loiter_radius = mavlink_mission_item->param3;
mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */
mission_item->loiter_exit_xtrack = (mavlink_mission_item->param4 > 0) ? true : false; mission_item->loiter_exit_xtrack = (mavlink_mission_item->param4 > 0) ? true : false;
break; break;
@ -940,8 +938,7 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
case MAV_CMD_NAV_LOITER_TO_ALT: case MAV_CMD_NAV_LOITER_TO_ALT:
mission_item->nav_cmd = NAV_CMD_LOITER_TO_ALT; mission_item->nav_cmd = NAV_CMD_LOITER_TO_ALT;
mission_item->force_heading = (mavlink_mission_item->param1 > 0) ? true : false; mission_item->force_heading = (mavlink_mission_item->param1 > 0) ? true : false;
mission_item->loiter_radius = fabsf(mavlink_mission_item->param2); mission_item->loiter_radius = mavlink_mission_item->param2;
mission_item->loiter_direction = (mavlink_mission_item->param2 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */
mission_item->loiter_exit_xtrack = (mavlink_mission_item->param4 > 0) ? true : false; mission_item->loiter_exit_xtrack = (mavlink_mission_item->param4 > 0) ? true : false;
break; break;
@ -1097,13 +1094,13 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
break; break;
case NAV_CMD_LOITER_UNLIMITED: case NAV_CMD_LOITER_UNLIMITED:
mavlink_mission_item->param3 = mission_item->loiter_radius * (float)mission_item->loiter_direction; mavlink_mission_item->param3 = mission_item->loiter_radius;
mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F; mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F;
break; break;
case NAV_CMD_LOITER_TIME_LIMIT: case NAV_CMD_LOITER_TIME_LIMIT:
mavlink_mission_item->param1 = mission_item->time_inside; mavlink_mission_item->param1 = mission_item->time_inside;
mavlink_mission_item->param3 = mission_item->loiter_radius * (float)mission_item->loiter_direction; mavlink_mission_item->param3 = mission_item->loiter_radius;
mavlink_mission_item->param4 = mission_item->loiter_exit_xtrack; mavlink_mission_item->param4 = mission_item->loiter_exit_xtrack;
break; break;
@ -1119,7 +1116,7 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
case NAV_CMD_LOITER_TO_ALT: case NAV_CMD_LOITER_TO_ALT:
mavlink_mission_item->param1 = mission_item->force_heading; mavlink_mission_item->param1 = mission_item->force_heading;
mavlink_mission_item->param2 = mission_item->loiter_radius * (float)mission_item->loiter_direction; mavlink_mission_item->param2 = mission_item->loiter_radius;
mavlink_mission_item->param4 = mission_item->loiter_exit_xtrack; mavlink_mission_item->param4 = mission_item->loiter_exit_xtrack;
break; break;

2
src/modules/navigator/datalinkloss.cpp
Unescape View File

@ -124,7 +124,6 @@ DataLinkLoss::set_dll_item()
_mission_item.altitude = _param_commsholdalt.get(); _mission_item.altitude = _param_commsholdalt.get();
_mission_item.yaw = NAN; _mission_item.yaw = NAN;
_mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = _param_commsholdwaittime.get() < 0.0f ? 0.0f : _param_commsholdwaittime.get(); _mission_item.time_inside = _param_commsholdwaittime.get() < 0.0f ? 0.0f : _param_commsholdwaittime.get();
@ -142,7 +141,6 @@ DataLinkLoss::set_dll_item()
_mission_item.altitude = _param_airfieldhomealt.get(); _mission_item.altitude = _param_airfieldhomealt.get();
_mission_item.yaw = NAN; _mission_item.yaw = NAN;
_mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
_mission_item.time_inside = _param_airfieldhomewaittime.get() < 0.0f ? 0.0f : _param_airfieldhomewaittime.get(); _mission_item.time_inside = _param_airfieldhomewaittime.get() < 0.0f ? 0.0f : _param_airfieldhomewaittime.get();
_mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.acceptance_radius = _navigator->get_acceptance_radius();

1
src/modules/navigator/enginefailure.cpp
Unescape View File

@ -113,7 +113,6 @@ EngineFailure::set_ef_item()
_mission_item.altitude = _navigator->get_home_position()->alt - 1000.0f; _mission_item.altitude = _navigator->get_home_position()->alt - 1000.0f;
_mission_item.yaw = NAN; _mission_item.yaw = NAN;
_mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.pitch_min = 0.0f; _mission_item.pitch_min = 0.0f;

1
src/modules/navigator/mission.cpp
Unescape View File

@ -1008,7 +1008,6 @@ Mission::do_abort_landing()
_mission_item.altitude = alt_sp; _mission_item.altitude = alt_sp;
_mission_item.yaw = NAN; _mission_item.yaw = NAN;
_mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.autocontinue = false; _mission_item.autocontinue = false;

16
src/modules/navigator/mission_block.cpp
Unescape View File

@ -207,7 +207,7 @@ MissionBlock::is_mission_item_reached()
* Therefore the item is marked as reached once the system reaches the loiter * Therefore the item is marked as reached once the system reaches the loiter
* radius (+ some margin). Time inside and turn count is handled elsewhere. * radius (+ some margin). Time inside and turn count is handled elsewhere.
*/ */
if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.loiter_radius * 1.2f) if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(fabsf(_mission_item.loiter_radius) * 1.2f)
&& dist_z <= _navigator->get_altitude_acceptance_radius()) { && dist_z <= _navigator->get_altitude_acceptance_radius()) {
_waypoint_position_reached = true; _waypoint_position_reached = true;
@ -235,7 +235,7 @@ MissionBlock::is_mission_item_reached()
_navigator->get_global_position()->alt, _navigator->get_global_position()->alt,
&dist_xy, &dist_z); &dist_xy, &dist_z);
if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.loiter_radius * 1.2f) if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(fabsf(_mission_item.loiter_radius) * 1.2f)
&& dist_z <= _navigator->get_altitude_acceptance_radius()) { && dist_z <= _navigator->get_altitude_acceptance_radius()) {
// now set the loiter to the final altitude in the NAV_CMD_LOITER_TO_ALT mission item // now set the loiter to the final altitude in the NAV_CMD_LOITER_TO_ALT mission item
@ -244,7 +244,7 @@ MissionBlock::is_mission_item_reached()
} }
} else { } else {
if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.loiter_radius * 1.2f) if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(fabsf(_mission_item.loiter_radius) * 1.2f)
&& dist_z <= _navigator->get_altitude_acceptance_radius()) { && dist_z <= _navigator->get_altitude_acceptance_radius()) {
_waypoint_position_reached = true; _waypoint_position_reached = true;
@ -483,9 +483,9 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
sp->lon = item->lon; sp->lon = item->lon;
sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->alt : item->altitude; sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->alt : item->altitude;
sp->yaw = item->yaw; sp->yaw = item->yaw;
sp->loiter_radius = (item->loiter_radius > NAV_EPSILON_POSITION) ? item->loiter_radius : sp->loiter_radius = (fabsf(item->loiter_radius) > NAV_EPSILON_POSITION) ? fabsf(item->loiter_radius) :
_navigator->get_loiter_radius(); _navigator->get_loiter_radius();
sp->loiter_direction = item->loiter_direction; sp->loiter_direction = (item->loiter_radius > 0) ? 1 : -1;
sp->pitch_min = item->pitch_min; sp->pitch_min = item->pitch_min;
sp->acceptance_radius = item->acceptance_radius; sp->acceptance_radius = item->acceptance_radius;
sp->disable_mc_yaw_control = false; sp->disable_mc_yaw_control = false;
@ -573,7 +573,6 @@ MissionBlock::set_loiter_item(struct mission_item_s *item, float min_clearance)
item->altitude_is_relative = false; item->altitude_is_relative = false;
item->yaw = NAN; item->yaw = NAN;
item->loiter_radius = _navigator->get_loiter_radius(); item->loiter_radius = _navigator->get_loiter_radius();
item->loiter_direction = 1;
item->acceptance_radius = _navigator->get_acceptance_radius(); item->acceptance_radius = _navigator->get_acceptance_radius();
item->time_inside = 0.0f; item->time_inside = 0.0f;
item->pitch_min = 0.0f; item->pitch_min = 0.0f;
@ -609,7 +608,6 @@ MissionBlock::set_follow_target_item(struct mission_item_s *item, float min_clea
item->altitude_is_relative = false; item->altitude_is_relative = false;
item->yaw = yaw; item->yaw = yaw;
item->loiter_radius = _navigator->get_loiter_radius(); item->loiter_radius = _navigator->get_loiter_radius();
item->loiter_direction = 1;
item->acceptance_radius = _navigator->get_acceptance_radius(); item->acceptance_radius = _navigator->get_acceptance_radius();
item->time_inside = 0.0f; item->time_inside = 0.0f;
item->pitch_min = 0.0f; item->pitch_min = 0.0f;
@ -631,7 +629,6 @@ MissionBlock::set_takeoff_item(struct mission_item_s *item, float abs_altitude,
item->yaw = NAN; item->yaw = NAN;
item->loiter_radius = _navigator->get_loiter_radius(); item->loiter_radius = _navigator->get_loiter_radius();
item->loiter_direction = 1;
item->time_inside = 0.0f; item->time_inside = 0.0f;
item->pitch_min = min_pitch; item->pitch_min = min_pitch;
item->autocontinue = false; item->autocontinue = false;
@ -673,7 +670,6 @@ MissionBlock::set_land_item(struct mission_item_s *item, bool at_current_locatio
item->altitude = 0; item->altitude = 0;
item->altitude_is_relative = false; item->altitude_is_relative = false;
item->loiter_radius = _navigator->get_loiter_radius(); item->loiter_radius = _navigator->get_loiter_radius();
item->loiter_direction = 1;
item->acceptance_radius = _navigator->get_acceptance_radius(); item->acceptance_radius = _navigator->get_acceptance_radius();
item->time_inside = 0.0f; item->time_inside = 0.0f;
item->pitch_min = 0.0f; item->pitch_min = 0.0f;
@ -691,7 +687,6 @@ MissionBlock::set_current_position_item(struct mission_item_s *item)
item->altitude = _navigator->get_global_position()->alt; item->altitude = _navigator->get_global_position()->alt;
item->yaw = NAN; item->yaw = NAN;
item->loiter_radius = _navigator->get_loiter_radius(); item->loiter_radius = _navigator->get_loiter_radius();
item->loiter_direction = 1;
item->acceptance_radius = _navigator->get_acceptance_radius(); item->acceptance_radius = _navigator->get_acceptance_radius();
item->time_inside = 0.0f; item->time_inside = 0.0f;
item->pitch_min = 0.0f; item->pitch_min = 0.0f;
@ -709,7 +704,6 @@ MissionBlock::set_idle_item(struct mission_item_s *item)
item->altitude = _navigator->get_home_position()->alt; item->altitude = _navigator->get_home_position()->alt;
item->yaw = NAN; item->yaw = NAN;
item->loiter_radius = _navigator->get_loiter_radius(); item->loiter_radius = _navigator->get_loiter_radius();
item->loiter_direction = 1;
item->acceptance_radius = _navigator->get_acceptance_radius(); item->acceptance_radius = _navigator->get_acceptance_radius();
item->time_inside = 0.0f; item->time_inside = 0.0f;
item->pitch_min = 0.0f; item->pitch_min = 0.0f;

19
src/modules/navigator/navigation.h
Unescape View File

@ -109,7 +109,7 @@ struct mission_item_s {
float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */ float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */
}; };
float acceptance_radius; /**< default radius in which the mission is accepted as reached in meters */ float acceptance_radius; /**< default radius in which the mission is accepted as reached in meters */
float loiter_radius; /**< loiter radius in meters, 0 for a VTOL to hover */ float loiter_radius; /**< loiter radius in meters, 0 for a VTOL to hover, negative for counter-clockwise */
float yaw; /**< in radians NED -PI..+PI, NAN means don't change yaw */ float yaw; /**< in radians NED -PI..+PI, NAN means don't change yaw */
float ___lat_float; /**< padding */ float ___lat_float; /**< padding */
float ___lon_float; /**< padding */ float ___lon_float; /**< padding */
@ -117,17 +117,18 @@ struct mission_item_s {
}; };
float params[7]; /**< array to store mission command values for MAV_FRAME_MISSION ***/ float params[7]; /**< array to store mission command values for MAV_FRAME_MISSION ***/
}; };
enum NAV_CMD nav_cmd; /**< navigation command */ uint16_t nav_cmd; /**< navigation command */
int16_t do_jump_mission_index; /**< index where the do jump will go to */ int16_t do_jump_mission_index; /**< index where the do jump will go to */
uint16_t do_jump_repeat_count; /**< how many times do jump needs to be done */ uint16_t do_jump_repeat_count; /**< how many times do jump needs to be done */
uint16_t do_jump_current_count; /**< count how many times the jump has been done */ uint16_t do_jump_current_count; /**< count how many times the jump has been done */
uint16_t origin; /**< where the waypoint has been generated */ struct {
bool loiter_exit_xtrack; /**< exit xtrack location: 0 for center of loiter wp, 1 for exit location */ uint16_t frame : 4, /**< mission frame ***/
bool force_heading; /**< heading needs to be reached ***/ origin : 3, /**< how the mission item was generated */
bool altitude_is_relative; /**< true if altitude is relative from start point */ loiter_exit_xtrack : 1, /**< exit xtrack location: 0 for center of loiter wp, 1 for exit location */
bool autocontinue; /**< true if next waypoint should follow after this one */ force_heading : 1, /**< heading needs to be reached ***/
int8_t frame; /**< mission frame ***/ altitude_is_relative : 1, /**< true if altitude is relative from start point */
int8_t loiter_direction; /**< 1: positive / clockwise, -1, negative. */ autocontinue : 1; /**< true if next waypoint should follow after this one */
};
}; };
#pragma pack(pop) #pragma pack(pop)
#include <uORB/topics/mission.h> #include <uORB/topics/mission.h>

1
src/modules/navigator/rcloss.cpp
Unescape View File

@ -114,7 +114,6 @@ RCLoss::set_rcl_item()
_mission_item.altitude_is_relative = false; _mission_item.altitude_is_relative = false;
_mission_item.yaw = NAN; _mission_item.yaw = NAN;
_mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = _param_loitertime.get() < 0.0f ? 0.0f : _param_loitertime.get(); _mission_item.time_inside = _param_loitertime.get() < 0.0f ? 0.0f : _param_loitertime.get();

4
src/modules/navigator/rtl.cpp
Unescape View File

@ -155,7 +155,6 @@ RTL::set_rtl_item()
_mission_item.altitude = climb_alt; _mission_item.altitude = climb_alt;
_mission_item.yaw = NAN; _mission_item.yaw = NAN;
_mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT; _mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f; _mission_item.time_inside = 0.0f;
@ -197,7 +196,6 @@ RTL::set_rtl_item()
} }
} }
_mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT; _mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f; _mission_item.time_inside = 0.0f;
@ -244,7 +242,6 @@ RTL::set_rtl_item()
} }
_mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT; _mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f; _mission_item.time_inside = 0.0f;
@ -269,7 +266,6 @@ RTL::set_rtl_item()
// don't change altitude // don't change altitude
_mission_item.yaw = _navigator->get_home_position()->yaw; _mission_item.yaw = _navigator->get_home_position()->yaw;
_mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED; _mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get(); _mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get();

Write Preview
Loading…
Cancel
Save