aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Mission: command specific structures

Browse Source
This commit is contained in:
Randy Mackay 2014-03-17 14:14:45 +09:00
parent 865a4de33d
commit c75560218d
2 changed files with 156 additions and 89 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

96
libraries/AP_Mission/AP_Mission.cpp
View File

@ -440,7 +440,7 @@ bool AP_Mission::mavlink_to_mission_cmd(const mavlink_mission_item_t& packet, AP
case MAV_CMD_NAV_LOITER_UNLIM: // MAV ID: 17 case MAV_CMD_NAV_LOITER_UNLIM: // MAV ID: 17
copy_location = true; copy_location = true;
cmd.content.location.flags.loiter_ccw = (packet.param3 < 0); cmd.content.location.flags.loiter_ccw = (packet.param3 < 0); // -1 = counter clockwise, +1 = clockwise
break; break;
case MAV_CMD_NAV_LOITER_TURNS: // MAV ID: 18 case MAV_CMD_NAV_LOITER_TURNS: // MAV ID: 18
@ -469,27 +469,27 @@ bool AP_Mission::mavlink_to_mission_cmd(const mavlink_mission_item_t& packet, AP
break; break;
case MAV_CMD_CONDITION_DELAY: // MAV ID: 112 case MAV_CMD_CONDITION_DELAY: // MAV ID: 112
cmd.content.location.lat = packet.param1; // delay in seconds cmd.content.delay.seconds = packet.param1; // delay in seconds
break; break;
case MAV_CMD_CONDITION_CHANGE_ALT: // MAV ID: 113 case MAV_CMD_CONDITION_CHANGE_ALT: // MAV ID: 113
copy_alt = true; // only altitude is used copy_alt = true; // only altitude is used
cmd.p1 = packet.param1; // climb/descent rate in m/s cmd.content.location.lat = packet.param1 * 100; // climb/descent converted from m/s to cm/s. To-Do: store in proper climb_rate structure
break; break;
case MAV_CMD_CONDITION_DISTANCE: // MAV ID: 114 case MAV_CMD_CONDITION_DISTANCE: // MAV ID: 114
cmd.content.location.lat = packet.param1; // distance in meters from next waypoint cmd.content.distance.meters = packet.param1; // distance in meters from next waypoint
break; break;
case MAV_CMD_CONDITION_YAW: // MAV ID: 115 case MAV_CMD_CONDITION_YAW: // MAV ID: 115
cmd.content.location.alt = packet.param1; // target angle in degrees cmd.content.yaw.angle_deg = packet.param1; // target angle in degrees
cmd.content.location.lat = packet.param2; // lat=0: use default turn rate otherwise specific turn rate in deg/sec cmd.content.yaw.turn_rate_dps = packet.param2; // 0 = use default turn rate otherwise specific turn rate in deg/sec
cmd.p1 = packet.param3; // -1 = ccw, +1 = cw cmd.content.yaw.direction = packet.param3; // -1 = ccw, +1 = cw
cmd.content.location.lng = packet.param4; // lng=0: absolute angle provided, lng=1: relative angle provided cmd.content.yaw.relative_angle = packet.param4; // lng=0: absolute angle provided, lng=1: relative angle provided
break; break;
case MAV_CMD_DO_SET_MODE: // MAV ID: 176 case MAV_CMD_DO_SET_MODE: // MAV ID: 176
cmd.p1 = packet.param1; // set flight mode. To-Do: make mapping function from MAVLINK defined flight modes to AC/AP/AR flight modes cmd.p1 = packet.param1; // flight mode identifier
break; break;
case MAV_CMD_DO_JUMP: // MAV ID: 177 case MAV_CMD_DO_JUMP: // MAV ID: 177
@ -498,9 +498,9 @@ bool AP_Mission::mavlink_to_mission_cmd(const mavlink_mission_item_t& packet, AP
break; break;
case MAV_CMD_DO_CHANGE_SPEED: // MAV ID: 178 case MAV_CMD_DO_CHANGE_SPEED: // MAV ID: 178
cmd.p1 = packet.param1; // 0 = airspeed, 1 = ground speed cmd.content.speed.speed_type = packet.param1; // 0 = airspeed, 1 = ground speed
cmd.content.location.alt = packet.param2; // speed in m/s cmd.content.speed.target_ms = packet.param2; // target speed in m/s
cmd.content.location.lat = packet.param3; // throttle as a percentage from 0 ~ 100% cmd.content.speed.throttle_pct = packet.param3; // throttle as a percentage from 0 ~ 100%
break; break;
case MAV_CMD_DO_SET_HOME: case MAV_CMD_DO_SET_HOME:
@ -514,26 +514,26 @@ bool AP_Mission::mavlink_to_mission_cmd(const mavlink_mission_item_t& packet, AP
break; break;
case MAV_CMD_DO_SET_RELAY: // MAV ID: 181 case MAV_CMD_DO_SET_RELAY: // MAV ID: 181
cmd.p1 = packet.param1; // relay number cmd.content.relay.num = packet.param1; // relay number
cmd.content.location.alt = packet.param2; // 0:off, 1:on cmd.content.relay.state = packet.param2; // 0:off, 1:on
break; break;
case MAV_CMD_DO_REPEAT_RELAY: // MAV ID: 182 case MAV_CMD_DO_REPEAT_RELAY: // MAV ID: 182
cmd.p1 = packet.param1; // relay number cmd.content.relay.num = packet.param1; // relay number
cmd.content.location.alt = packet.param2; // count cmd.content.relay.repeat_count = packet.param2; // count
cmd.content.location.lat = packet.param3*1000; // time cmd.content.relay.time_ms = packet.param3*1000; // time converted from seconds to milliseconds
break; break;
case MAV_CMD_DO_SET_SERVO: // MAV ID: 183 case MAV_CMD_DO_SET_SERVO: // MAV ID: 183
cmd.p1 = packet.param1; // channel cmd.content.servo.channel = packet.param1; // channel
cmd.content.location.alt = packet.param2; // PWM cmd.content.servo.pwm = packet.param2; // PWM
break; break;
case MAV_CMD_DO_REPEAT_SERVO: // MAV ID: 184 case MAV_CMD_DO_REPEAT_SERVO: // MAV ID: 184
cmd.p1 = packet.param1; // channel cmd.content.servo.channel = packet.param1; // channel
cmd.content.location.alt = packet.param2; // PWM cmd.content.servo.pwm = packet.param2; // PWM
cmd.content.location.lat = packet.param3; // count cmd.content.servo.repeat_count = packet.param3; // count
cmd.content.location.lng = packet.param4*1000; // time in seconds converted to milliseconds cmd.content.servo.time_ms = packet.param4*1000; // time in seconds converted to milliseconds
break; break;
case MAV_CMD_DO_SET_ROI: // MAV ID: 201 case MAV_CMD_DO_SET_ROI: // MAV ID: 201
@ -547,7 +547,7 @@ bool AP_Mission::mavlink_to_mission_cmd(const mavlink_mission_item_t& packet, AP
break; break;
case MAV_CMD_DO_SET_CAM_TRIGG_DIST: // MAV ID: 206 case MAV_CMD_DO_SET_CAM_TRIGG_DIST: // MAV ID: 206
cmd.content.location.alt = packet.param1; // use alt so we can support 32 bit values cmd.content.cam_trigg_dist.meters = packet.param1; // distance between camera shots in meters
break; break;
default: default:
@ -668,27 +668,27 @@ bool AP_Mission::mission_cmd_to_mavlink(const AP_Mission::Mission_Command& cmd,
break; break;
case MAV_CMD_CONDITION_DELAY: // MAV ID: 112 case MAV_CMD_CONDITION_DELAY: // MAV ID: 112
packet.param1 = cmd.content.location.lat; // delay in seconds packet.param1 = cmd.content.delay.seconds; // delay in seconds
break; break;
case MAV_CMD_CONDITION_CHANGE_ALT: // MAV ID: 113 case MAV_CMD_CONDITION_CHANGE_ALT: // MAV ID: 113
copy_alt = true; // only altitude is used copy_alt = true; // only altitude is used
packet.param1 = cmd.p1/100; // climb/descent rate converted from m/s to cm/s packet.param1 = cmd.content.location.lat / 100.0f; // climb/descent rate converted from cm/s to m/s. To-Do: store in proper climb_rate structure
break; break;
case MAV_CMD_CONDITION_DISTANCE: // MAV ID: 114 case MAV_CMD_CONDITION_DISTANCE: // MAV ID: 114
packet.param1 = cmd.content.location.lat; // distance in meters from next waypoint packet.param1 = cmd.content.distance.meters; // distance in meters from next waypoint
break; break;
case MAV_CMD_CONDITION_YAW: // MAV ID: 115 case MAV_CMD_CONDITION_YAW: // MAV ID: 115
packet.param1 = cmd.content.location.alt; // target angle in degrees packet.param1 = cmd.content.yaw.angle_deg; // target angle in degrees
packet.param2 = cmd.content.location.lat; // lat=0: use default turn rate otherwise specific turn rate in deg/sec packet.param2 = cmd.content.yaw.turn_rate_dps; // 0 = use default turn rate otherwise specific turn rate in deg/sec
packet.param3 = cmd.p1; // -1 = ccw, +1 = cw packet.param3 = cmd.content.yaw.direction; // -1 = ccw, +1 = cw
packet.param4 = cmd.content.location.lng; // lng=0: absolute angle provided, lng=1: relative angle provided packet.param4 = cmd.content.yaw.relative_angle; // 0 = absolute angle provided, 1 = relative angle provided
break; break;
case MAV_CMD_DO_SET_MODE: // MAV ID: 176 case MAV_CMD_DO_SET_MODE: // MAV ID: 176
packet.param1 = cmd.p1; // set flight mode. To-Do: make mapping function from MAVLINK defined flight modes to AC/AP/AR flight modes packet.param1 = cmd.p1; // set flight mode identifier
break; break;
case MAV_CMD_DO_JUMP: // MAV ID: 177 case MAV_CMD_DO_JUMP: // MAV ID: 177
@ -697,9 +697,9 @@ bool AP_Mission::mission_cmd_to_mavlink(const AP_Mission::Mission_Command& cmd,
break; break;
case MAV_CMD_DO_CHANGE_SPEED: // MAV ID: 178 case MAV_CMD_DO_CHANGE_SPEED: // MAV ID: 178
packet.param1 = cmd.p1; // 0 = airspeed, 1 = ground speed packet.param1 = cmd.content.speed.speed_type; // 0 = airspeed, 1 = ground speed
packet.param2 = cmd.content.location.alt; // speed in m/s packet.param2 = cmd.content.speed.target_ms; // speed in m/s
packet.param3 = cmd.content.location.lat; // throttle as a percentage from 0 ~ 100% packet.param3 = cmd.content.speed.throttle_pct; // throttle as a percentage from 0 ~ 100%
break; break;
case MAV_CMD_DO_SET_HOME: // MAV ID: 179 case MAV_CMD_DO_SET_HOME: // MAV ID: 179
@ -713,26 +713,26 @@ bool AP_Mission::mission_cmd_to_mavlink(const AP_Mission::Mission_Command& cmd,
break; break;
case MAV_CMD_DO_SET_RELAY: // MAV ID: 181 case MAV_CMD_DO_SET_RELAY: // MAV ID: 181
packet.param1 = cmd.p1; // relay number packet.param1 = cmd.content.relay.num; // relay number
packet.param2 = cmd.content.location.alt; // 0:off, 1:on packet.param2 = cmd.content.relay.state; // 0:off, 1:on
break; break;
case MAV_CMD_DO_REPEAT_RELAY: // MAV ID: 182 case MAV_CMD_DO_REPEAT_RELAY: // MAV ID: 182
packet.param1 = cmd.p1; // relay number packet.param1 = cmd.content.relay.num; // relay number
packet.param2 = cmd.content.location.alt; // count packet.param2 = cmd.content.relay.repeat_count; // count
packet.param3 = cmd.content.location.lat*0.001f;// time packet.param3 = cmd.content.relay.time_ms*0.001f; // time converted from milliseconds to seconds
break; break;
case MAV_CMD_DO_SET_SERVO: // MAV ID: 183 case MAV_CMD_DO_SET_SERVO: // MAV ID: 183
packet.param1 = cmd.p1; // channel packet.param1 = cmd.content.servo.channel; // channel
packet.param2 = cmd.content.location.alt; // PWM packet.param2 = cmd.content.servo.pwm; // PWM
break; break;
case MAV_CMD_DO_REPEAT_SERVO: // MAV ID: 184 case MAV_CMD_DO_REPEAT_SERVO: // MAV ID: 184
packet.param1 = cmd.p1; // channel packet.param1 = cmd.content.servo.channel; // channel
packet.param2 = cmd.content.location.alt; // PWM packet.param2 = cmd.content.servo.pwm; // PWM
packet.param3 = cmd.content.location.lat; // count packet.param3 = cmd.content.servo.repeat_count; // count
packet.param4 = cmd.content.location.lng*0.001f;// time in seconds converted to milliseconds packet.param4 = cmd.content.servo.time_ms*0.001f; // time in milliseconds converted to seconds
break; break;
case MAV_CMD_DO_SET_ROI: // MAV ID: 201 case MAV_CMD_DO_SET_ROI: // MAV ID: 201
@ -746,7 +746,7 @@ bool AP_Mission::mission_cmd_to_mavlink(const AP_Mission::Mission_Command& cmd,
break; break;
case MAV_CMD_DO_SET_CAM_TRIGG_DIST: // MAV ID: 206 case MAV_CMD_DO_SET_CAM_TRIGG_DIST: // MAV ID: 206
packet.param1 = cmd.content.location.alt; // use alt so we can support 32 bit values packet.param1 = cmd.content.cam_trigg_dist.meters; // distance between camera shots in meters
break; break;
default: default:
@ -769,7 +769,7 @@ bool AP_Mission::mission_cmd_to_mavlink(const AP_Mission::Mission_Command& cmd,
} }
} }
// if we got this far then it must have been succesful // if we got this far then it must have been successful
return true; return true;
} }

77
libraries/AP_Mission/AP_Mission.h
View File

@ -42,21 +42,88 @@ class AP_Mission {
public: public:
// jump command structure // jump command structure
struct Jump_Command { struct PACKED Jump_Command {
uint16_t target; // DO_JUMP target command id uint16_t target; // target command id
int16_t num_times; // DO_JUMP num times to repeat. -1 = repeat forever int16_t num_times; // num times to repeat. -1 = repeat forever
}; };
union Content { // condition delay command structure
struct PACKED Conditional_Delay_Command {
uint16_t seconds; // period of delay in seconds
};
// condition delay command structure
struct PACKED Conditional_Distance_Command {
int32_t meters; // distance from next waypoint in meters
};
// condition yaw command structure
struct PACKED Yaw_Command {
int16_t angle_deg; // target angle in degrees (0=north, 90=east)
int16_t turn_rate_dps; // turn rate in degrees / second (0=use default)
int8_t direction; // -1 = ccw, +1 = cw
uint8_t relative_angle;// 0 = absolute angle, 1 = relative angle
};
// change speed command structure
struct PACKED Change_Speed_Command {
uint8_t speed_type; // 0=airspeed, 1=ground speed
float target_ms; // target speed in m/s
uint8_t throttle_pct; // throttle as a percentage (i.e. 0 ~ 100)
};
// set relay and repeat relay command structure
struct PACKED Set_Relay_Command {
uint8_t num; // relay number from 1 to 4
uint8_t state; // on = 3.3V or 5V (depending upon board), off = 0V. only used for do-set-relay, not for do-repeat-relay
int16_t repeat_count; // number of times to trigger the relay
uint32_t time_ms; // cycle time in milliseconds (the time between peaks or the time the relay is on and off for each cycle?)
};
// set servo and repeat servo command structure
struct PACKED Set_Servo_Command {
uint8_t channel; // Note: p1 holds servo channel
uint16_t pwm; // pwm value for servo
int16_t repeat_count; // number of times to move the servo (returns to trim in between)
uint16_t time_ms; // cycle time in milliseconds (the time between peaks or the time the servo is at the specified pwm value for each cycle?)
};
// set cam trigger distance command structure
struct PACKED Cam_Trigg_Distance {
int32_t meters; // distance
};
union PACKED Content {
// jump structure // jump structure
Jump_Command jump; Jump_Command jump;
// conditional delay
Conditional_Delay_Command delay;
// conditional distance
Conditional_Distance_Command distance;
// conditional yaw
Yaw_Command yaw;
// change speed
Change_Speed_Command speed;
// do-set-relay and do-repeat-relay
Set_Relay_Command relay;
// do-set-servo and do-repeate-servo
Set_Servo_Command servo;
// cam trigg distance
Cam_Trigg_Distance cam_trigg_dist;
// location // location
Location location; // Waypoint location Location location; // Waypoint location
}; };
// command structure // command structure
struct Mission_Command { struct PACKED Mission_Command {
uint16_t index; // this commands position in the command list uint16_t index; // this commands position in the command list
uint8_t id; // mavlink command id uint8_t id; // mavlink command id
uint8_t p1; // general purpose parameter 1 uint8_t p1; // general purpose parameter 1